#include "trilat.h"

namespace Lio {

static const GLfloat
	LightAmbient0[]=	{ 0.5f, 0.5f, 0.5f, 1.0f }, // Ambient Light Values
	LightDiffuse0[]=	{ 1.0f, 1.0f, 1.0f, 1.0f }, // Diffuse Light Values
	LightPosition0[]=	{ 0.0f, 0.0f, 2.0f, 1.0f }; // Light Position

CSTR Trilat::nameActions[ACTION_COUNT]={
	_T("None"),
	_T("Map"),
	_T("Fit"),
	_T("Script"),
	_T("Graph"),
	_T("Plan"),
};
CSTR Trilat::nameOrder[ORDER_COUNT]={
	_T("None"),
	_T("Random"),
	_T("Circle"),
	_T("Line")
};
CSTR Trilat::nameMaps[MAP_COUNT]={
	_T("None"),
	_T("est Distrib"),
	_T("num Distrib"),
	_T("err Distrib"),
	_T("num Error"),
	_T("Trans Fact"),
	_T("Trans Full"),
	_T("est Var"),
	_T("est VarX"),
	_T("est VarY"),
	_T("est Correl"),
	_T("par Var"),
	_T("par Error"),
	_T("Mass"),
	_T("Fullmass"),
	_T("Grad Mass"),
	_T("Iters"),
	_T("Iter miss"),
	_T("diff Var"),
	_T("diff Correl"),
	_T("num Var"),
	_T("num VarX"),
	_T("num VarY"),
	_T("num Correl"),
	_T("Mean"),
	_T("MeanX"),
	_T("MeanY"),
	_T("Excess"),
	_T("ExcessX"),
	_T("ExcessY"),
	_T("Skew"),
	_T("SkewX"),
	_T("SkewY"),
	_T("Energy"),
	_T("Grad Energy"),
	_T("ln(MLK)")
};
CSTR Trilat::nameModels[MODEL_COUNT]={
	_T("TOF"),
	_T("sTOF"),
	_T("TDOA"),
	_T("sTDOA"),
	_T("sTDOA2"),
	_T("ITER_A"),
	_T("ITER_A2"),
	_T("ITER_G"),
	_T("ITER_Q")
};
CSTR Trilat::nameSect[SECT_COUNT]={
	_T("None"),
	_T("R->MapMin"),
	_T("R->MapMax"),
	_T("R->VarMin"),
	_T("R->Active"),
	_T("Min->Max"),
	_T("Custom")
};
CSTR Trilat::nameInfos[INFO_COUNT]={
	_T("None"),
	_T("Num"),
	_T("Coor"),
	_T("Dist"),
	_T("Trans"),
	_T("Mass"),
	_T("GradM"),
	_T("Opt")
};
CSTR Trilat::nameGraphs[GRAPH_COUNT]={
	_T("Section"),
	_T("Density"),
	_T("Custom"),
	_T("Route"),
};
CSTR Trilat::nameOpts[OPT_COUNT]={
	_T("None"),
	_T("Trace"),
	_T("Det"),
};
CSTR Trilat::nameVars[MAP_ARG_COUNT]={
	_T("Point"),
	_T("Beacon"),
	_T("Start"),
};
CSTR Trilat::namePlans[PLAN_TYPE_COUNT]={
	_T("Sim"),
	_T("Once"),
	_T("All"),
};
CSTR Trilat::nameShow[POS_COUNT]={
	_T("target"),	// POS_TARGET
	_T("start"),	// POS_START
	_T("end"),		// POS_END
	_T("errmin"),	// POS_ERRMIN
	_T("iter"),		// POS_ITER
	_T("min"),		// POS_LEVMIN
	_T("max")		// POS_LEVMAX
};
CSTR Trilat::nameResults[ACTION_ERROR_COUNT]={
	"success", // ACTION_ERROR_NONE,
	"interrupted", // ACTION_ERROR_INTERRUPT,
	"stopped", //ACTION_ERROR_STOPPED
	"deprecated", // ACTION_ERROR_OLD,
	"init error" //ACTION_ERROR_INIT,
};
CSTR Trilat::nameScripts[SCRIPT_COUNT]={
	_T("num est"),//SCRIPT_NUM_EST
	_T("sym beacon"),//SCRIPT_SYM_BEACON
	_T("stat change XY"),//SCRIPT_STAT_CHANGE_XY
	_T("stat change L")//SCRIPT_STAT_CHANGE_L
};
Trilat::ChnlInfo Trilat::chnlInfos[CHNL_COUNT]={
//	name					X					color			thick	pattern		hidden symbol
	{_T("Section length"),	CHNL_NONE,			0,				1.0f,	0x0000ffff,	TRUE,	_T("")},// CHNL_SECT_ARG
	{_T("Section value"),	CHNL_SECT_ARG,		INT_BLUE,		0.0f,	0x0000ffff,	TRUE,	_T("")},// CHNL_SECT_VAL
	{_T("Section LSM"),		CHNL_SECT_ARG,		INT_DBLUE,		2.0f,	0x0000ff00,	TRUE,	_T("")},// CHNL_SECT_LSM
	{_T("X value"),			CHNL_NONE,			0,				1.0f,	0x0000ffff,	TRUE,	_T("")},// CHNL_DENS_X_ARG
	{_T("Y value"),			CHNL_NONE,			0,				1.0f,	0x0000ffff,	TRUE,	_T("")},// CHNL_DENS_Y_ARG
	{_T("L value"),			CHNL_NONE,			0,				1.0f,	0x0000ffff,	TRUE,	_T("")},// CHNL_DENS_L_ARG
	{_T("X dens est"),		CHNL_DENS_X_ARG,	INT_DBLUE,		1.5f,	0x0000ff00,	TRUE,	_T("estX")},// CHNL_DENS_X_EST
	{_T("Y dens est"),		CHNL_DENS_Y_ARG,	INT_DRED,		1.5f,	0x0000ff00,	TRUE,	_T("estY")},// CHNL_DENS_Y_EST
	{_T("L dens est"),		CHNL_DENS_L_ARG,	INT_DGREEN,		1.5f,	0x0000ff00,	TRUE,	_T("est")},// CHNL_DENS_L_EST
	{_T("X dens num"),		CHNL_DENS_X_ARG,	INT_DBLUE,		0.0f,	0x0000ffff,	TRUE,	_T("numX")},// CHNL_DENS_X_NUM
	{_T("Y dens num"),		CHNL_DENS_Y_ARG,	INT_DRED,		0.0f,	0x0000ffff,	TRUE,	_T("numY")},// CHNL_DENS_Y_NUM
	{_T("L dens num"),		CHNL_DENS_L_ARG,	INT_DGREEN,		0.0f,	0x0000ffff,	TRUE,	_T("num")},// CHNL_DENS_L_NUM
	{_T("X dens dif"),		CHNL_DENS_X_ARG,	INT_DBLUE,		1.5f,	0x0000ffff,	TRUE,	_T("errX")},// CHNL_DENS_X_DIF
	{_T("Y dens dif"),		CHNL_DENS_Y_ARG,	INT_DRED,		1.5f,	0x0000ffff,	TRUE,	_T("errY")},// CHNL_DENS_Y_DIF
	{_T("L dens dif"),		CHNL_DENS_L_ARG,	INT_DGREEN,		1.5f,	0x0000ffff,	TRUE,	_T("err")},// CHNL_DENS_L_DIF
	{_T("Custom arg"),		CHNL_NONE,			0,				1.0f,	0x0000ffff,	TRUE,	_T("")},// CHNL_CUST_ARG
	{_T("Stdev est x"),		CHNL_CUST_ARG,		INT_DRED,		1.5f,	0x0000ff00,	TRUE,	_T("estStdevX")},// CHNL_CUST_STDEV_EST_X
	{_T("Stdev est y"),		CHNL_CUST_ARG,		INT_DBLUE,		1.5f,	0x0000ff00,	TRUE,	_T("estStdevY")},// CHNL_CUST_STDEV_EST_Y
	{_T("Stdev est"),		CHNL_CUST_ARG,		INT_DGRAY,		1.5f,	0x0000ff00,	TRUE,	_T("estStdev")},// CHNL_CUST_STDEV_EST
	{_T("Correl est"),		CHNL_CUST_ARG,		INT_DGREEN,		1.5f,	0x0000ff00,	TRUE,	_T("estCorrel")},// CHNL_CUST_CORREL_EST
	{_T("Stdev num x"),		CHNL_CUST_ARG,		INT_RED,		1.0f,	0x0000ffff,	TRUE,	_T("numStdevX")},// CHNL_CUST_STDEV_NUM_X
	{_T("Stdev num y"),		CHNL_CUST_ARG,		INT_BLUE,		1.0f,	0x0000ffff,	TRUE,	_T("numStdevY")},// CHNL_CUST_STDEV_NUM_Y
	{_T("Stdev num"),		CHNL_CUST_ARG,		INT_GRAY,		1.0f,	0x0000ffff,	TRUE,	_T("numStdev")},// CHNL_CUST_STDEV_NUM
	{_T("Correl num"),		CHNL_CUST_ARG,		INT_DGREEN,		1.0f,	0x0000ffff,	TRUE,	_T("numCorrel")},// CHNL_CUST_CORREL_NUM
	{_T("Stdev err x"),		CHNL_CUST_ARG,		INT_DMAGENTA,	1.0f,	0x0000ffff,	TRUE,	_T("errStdevX")},// CHNL_CUST_STDEV_DIF_X
	{_T("Stdev err y"),		CHNL_CUST_ARG,		INT_DCYAN,		1.0f,	0x0000ffff,	TRUE,	_T("errStdevY")},// CHNL_CUST_STDEV_DIF_Y
	{_T("Stdev err"),		CHNL_CUST_ARG,		INT_BLACK,		0.0f,	0x0000ffff,	TRUE,	_T("errStdev")},// CHNL_CUST_STDEV_DIF
	{_T("Correl err"),		CHNL_CUST_ARG,		INT_DYELLOW,	1.0f,	0x0000ffff,	TRUE,	_T("errCorrel")},// CHNL_CUST_CORREL_DIF
	{_T("Par var err"),		CHNL_CUST_ARG,		INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("parVarErr")},//CHNL_CUST_STDEV_PAR_DIF
	{_T("Mean X"),			CHNL_CUST_ARG,		INT_RED,		1.0f,	0x0000ffff,	TRUE,	_T("meanX")},// CHNL_CUST_MEAN_X
	{_T("Mean Y"),			CHNL_CUST_ARG,		INT_BLUE,		1.0f,	0x0000ffff,	TRUE,	_T("meanY")},// CHNL_CUST_MEAN_Y
	{_T("Mean"),			CHNL_CUST_ARG,		INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("mean")},// CHNL_CUST_MEAN
	{_T("Excess X"),		CHNL_CUST_ARG,		INT_RED,		1.0f,	0x0000ffff,	TRUE,	_T("excessX")},// CHNL_CUST_EXCESS_X
	{_T("Excess Y"),		CHNL_CUST_ARG,		INT_BLUE,		1.0f,	0x0000ffff,	TRUE,	_T("excessY")},// CHNL_CUST_EXCESS_Y
	{_T("Excess"),			CHNL_CUST_ARG,		INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("excess")},// CHNL_CUST_EXCESS
	{_T("Skew X"),			CHNL_CUST_ARG,		INT_RED,		1.0f,	0x0000ffff,	TRUE,	_T("skewX")},// CHNL_CUST_SKEW_X
	{_T("Skew Y"),			CHNL_CUST_ARG,		INT_BLUE,		1.0f,	0x0000ffff,	TRUE,	_T("skewY")},// CHNL_CUST_SKEW_Y
	{_T("Skew"),			CHNL_CUST_ARG,		INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("skew")},// CHNL_CUST_SKEW
	{_T("Weight"),			CHNL_CUST_ARG,		INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("m")},// CHNL_CUST_MASS
	{_T("FGW"),				CHNL_CUST_ARG,		INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("M")},// CHNL_CUST_FULL_MASS
	{_T("Min Var"),			CHNL_CUST_ARG,		INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("minVar")},// CHNL_CUST_MIN_VAR
	{_T("detU"),			CHNL_CUST_ARG,		INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("detU")},// CHNL_CUST_AVG_RAD
	{_T("ux"),				CHNL_CUST_ARG,		INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("ux")},// CHNL_CUST_UX
	{_T("uy"),				CHNL_CUST_ARG,		INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("uy")},// CHNL_CUST_UY
	{_T("uxy"),				CHNL_CUST_ARG,		INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("uxy")},// CHNL_CUST_UXY
	{_T("map min"),			CHNL_CUST_ARG,		INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("min")},// CHNL_CUST_LEV_MIN
	{_T("map max"),			CHNL_CUST_ARG,		INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("max")},// CHNL_CUST_LEV_MAX
	{_T("map mid"),			CHNL_CUST_ARG,		INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("mid")},// CHNL_CUST_LEV_MID
	{_T("Calc time"),		CHNL_CUST_ARG,		INT_BLACK,		0.0f,	0x0000ffff,	TRUE,	_T("time")}, // CHNL_CUST_TIME,
	{_T("Iter Route"),		CHNL_ITER_COUNT,	INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("route")},// CHNL_ITER_ROUTE
	{_T("Iter Count"),		CHNL_ITER_ROUTE,	INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("count")},// CHNL_ITER_COUNT
	{_T("Iter Step"),		CHNL_ITER_COUNT,	INT_BLUE,		1.0f,	0x0000ffff,	TRUE,	_T("step")},// CHNL_ITER_STEP
	{_T("Iter Trig"),		CHNL_ITER_COUNT,	INT_DGRAY,		1.0f,	0x0000ffff,	TRUE,	_T("trig")},// CHNL_ITER_TRIG
	{_T("Iter Error"),		CHNL_ITER_COUNT,	INT_RED,		1.0f,	0x0000ffff,	TRUE,	_T("error")},// CHNL_ITER_ERROR
	{_T("Iter Var"),		CHNL_ITER_COUNT,	INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("var")},// CHNL_ITER_VAR
	{_T("Iter Func E,FGW"),	CHNL_ITER_COUNT,	INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("func")},// CHNL_ITER_FUNC
	{_T("Iter Grad"),		CHNL_ITER_COUNT,	INT_BLACK,		1.0f,	0x0000ffff,	TRUE,	_T("grad")},// CHNL_ITER_GRAD
	{_T("Iter GradX"),		CHNL_ITER_COUNT,	INT_RED,		1.0f,	0x0000ffff,	TRUE,	_T("gradX")},// CHNL_ITER_GRAD_X
	{_T("Iter GradY"),		CHNL_ITER_COUNT,	INT_BLUE,		1.0f,	0x0000ffff,	TRUE,	_T("gradY")} // CHNL_ITER_GRAD_Y
};
BOOL Trilat::setArray(int newBeacons, int newSamples, BOOL keep)
{
	int i,oldN=N,oldK=K;
	Vector<Point> tmpB;
	Lock lock(&draw_sect);

	if(newBeacons<1)
		newBeacons=1;
	if(newSamples>=0&&K!=newSamples)
	{
		if(newSamples<3)
			kSample=newSamples=0;
		else
			kSample=INV(newSamples);
		if(!(distribs=vDistrib.setSize(newSamples)))
			goto ERROR_SET_ARRAY;
		K=newSamples;
	}
	if(N!=newBeacons)
	{
		tmpB.copy(vB);
		if(	!(B=vB.setSize(newBeacons))||
			!(stats=vStats.setSize(newBeacons))||
			!(M=vM.setSize(newBeacons))||
			!(gradM=vGradM.setSize(newBeacons))||
			!(R2=vR2.setSize(newBeacons))||
			!(O=vO.setSize(newBeacons))||
			!(mP=vP.setSize(newBeacons))||
			!(mJ=vJ.setSize(newBeacons))||
			
			!(Lr=vL.setSize(newBeacons))||
			!(Le=vLe.setSize(newBeacons))||

			!(D=mD.setSize(newBeacons,1))||
			!(A=mA.setSize(newBeacons,3))||
			!(Ea=mEa.setSize(newBeacons,newBeacons))||
			!(invA=mInvA.setSize(3,newBeacons))||
			!(A0=mA0.setSize(newBeacons,3))||
			!(invA0=mInvA0.setSize(3,newBeacons))||
			!(At=mAt.setSize(3,newBeacons))||
			!(F=mF.setSize(newBeacons,3))||
			!(invF=invertF.bind(&mF))||
			!(V=mV.setSize(newBeacons,1)))
			goto ERROR_SET_ARRAY;

		N=newBeacons;
		kBeacons=INV(N);
		chActive.clear();
		String str(64);
		for(i=0;i<N;i++)
		{
			if(	!mJ[i].setSize(2,3)||
				!mP[i].setSize(3,3))
				goto ERROR_SET_ARRAY;
			mP[i].clear();
			if(i<oldN)
				B[i].copy(tmpB[i]);
			else if(!locateBeacons())
				B[i].set(0.5*areaSide*(1+random(-1,1)),0.5*areaSide*(1+random(-1,1)));
			chActive.add(str.print(_T("Beacon %d"),i+1));
		}
		active=chActive.setSelectedIndex(0);
		updateModel();
		buildStat();
	}
	if(!keep)
		errors=NULL;
	else if(tErrors.cols<N||tErrors.rows<K)
		errors=tErrors.setSize(K,N);
	keepErrors=keep&&errors;
	if(!keepErrors)
		tErrors.destroy();
	tbCount.setInt(N);
	tbSamples.setInt(K);
	bKeepErrors.checked=keepErrors;
	return TRUE;
ERROR_SET_ARRAY:
	N=oldN;
	K=oldK;
	reset();
	return FALSE;
}
void Trilat::setModel(int newModel)
{
	if(newModel<0||newModel>=MODEL_COUNT)
		newModel=MODEL_S_TOF;
	model=newModel;
	route.destroy();
	chModel.setSelectedIndex(newModel);
	updateModel();
}
void Trilat::updateModel()
{
	switch(model)
	{
	case MODEL_TOF:
		cols=2;
		rows=N-1;
		break;
	case MODEL_S_TOF:
		cols=2;
		rows=N;
		break;
	case MODEL_TDOA:
		cols=3;
		rows=N-1;
		break;
	case MODEL_S_TDOA:
		cols=2;
		rows=N;
		break;
	case MODEL_S_TDOA2:
		cols=3;
		rows=N;
		break;
	default:
		cols=0;
		rows=0;
		break;
	}
}
void Trilat::get3DWebStep(BOOL searchDown)
{
	int a=areaSide-1;
	if(count3DWeb>1)
	{
		if(count3DWeb<a)
		{
			if(searchDown)
				while(a%count3DWeb!=0)
					count3DWeb--;
			else
				while(a%count3DWeb!=0)
					count3DWeb++;
		}
		else
			count3DWeb=a;
	}
	else
		count3DWeb=1;
	step3DWeb=a/count3DWeb;
}

#define ADDVERTEX(i) glColor3fv(buff3D[i].rgb); glNormal3fv(buff3D[i].normal); glVertex3fv(buff3D[i].vertex);
void Trilat::compile3D()
{
	int i,j,w,h;

	if(color3D)
	{
		for(i=0;i<areaSize;i++)
			colorHSLtoRGB(buff3D[i].hsl,buff3D[i].rgb);
	}
	else
	{
		float *rgb,lum;
		for(i=0;i<areaSize;i++)
		{
			rgb=buff3D[i].rgb;
			lum=buff3D[i].hsl[LUM];
			rgb[RED]=lum;
			rgb[GREEN]=lum;
			rgb[BLUE]=lum;
		}
	}

	CompileDisplayList compile(&list3D);
	glBegin(GL_QUADS);
	for(h=1;h<areaSide;h++)
	{
		for(w=1;w<areaSide;w++)
		{
			i=h*areaSide+w;
			j=(h-1)*areaSide+w;
			ADDVERTEX(i);
			ADDVERTEX(i-1);
			ADDVERTEX(j-1);
			ADDVERTEX(j);
		}
	}
	glEnd();
}

void Trilat::set3D(BOOL value)
{
	container.visible=!(mode3D=value);

	int w,h,i;
	double level,tmp;
	float *va,*na,*v[4],n[4][3];
	Data3D *act;
	BOOL lim=bMapLims.checked;

	if(!mode3D)
		return;
	oldMousePos.set(0,0);
	get3DWebStep(TRUE);
	double tmp1=INV(mapmax-mapmin),tmp2=INV(levmax-levmin);
	for(h=0,i=0;h<areaSide;h++)
	{
		for(w=0;w<areaSide;w++,i++)
		{
			level=levels[i];
			if(level==ERROR_LEVEL)
				level=0;
			act=buff3D+i;
			act->vertex[0]=remap(w,0,areaSide-1,-1.0f,1.0f);
			act->vertex[1]=remap(h,0,areaSide-1,-1.0f,1.0f);
			if(lim)
			{
				if(level>=mapmax)
					tmp=1.0;
				else if(level<=mapmin)
					tmp=0.0;
				else
					tmp=(level-mapmin)*tmp1;
			}
			else
			{
				tmp=(level-levmin)*tmp2;
			}
			act->vertex[2]=float(tmp);
			act->hsl[HUE]=float(0.6*(1-tmp));
			act->hsl[SAT]=1.0f;
			act->hsl[LUM]=float(0.1+tmp*0.5);
		}
	}
	for(w=1;w<areaSide-1;w++)
	{
		// bottom side normals: h=0,w>0 and w<areaSide-1
		//    1
		//  / | \
		// 2 -a- 0
		// 
		// 
		i=w;
		na=buff3D[i].normal;
		va=buff3D[i].vertex;
		v[0]=buff3D[i+1].vertex;
		v[1]=buff3D[i+areaSide].vertex;
		v[2]=buff3D[i-1].vertex;
		getNormalVector(va,v[1],v[0],n[0]);
		getNormalVector(va,v[2],v[1],n[1]);
		na[0]=n[0][0]+n[1][0];
		na[1]=n[0][1]+n[1][1];
		na[2]=n[0][2]+n[1][2];
		getUnitVector(na);
	}
	for(h=1;h<areaSide-1;h++)
	{
		// left side normals: w=0,h>0 and h<areaSide-1
		//    1
		//    | \
		//    a- 0
		//    | /
		//    3
		i=h*areaSide;
		na=buff3D[i].normal;
		va=buff3D[i].vertex;
		v[0]=buff3D[i+1].vertex;
		v[1]=buff3D[i+areaSide].vertex;
		v[3]=buff3D[i-areaSide].vertex;
		getNormalVector(va,v[1],v[0],n[0]);
		getNormalVector(va,v[0],v[3],n[3]);
		na[0]=n[0][0]+n[3][0];
		na[1]=n[0][1]+n[3][1];
		na[2]=n[0][2]+n[3][2];
		getUnitVector(na);
		for(w=1;w<areaSide-1;w++)
		{
			// normals: w>0,h>0,w<areaSide-1 and h<areaSide-1
			//    1
			//  / | \
			// 2 -a- 0
			//  \ | /
			//    3
			i=h*areaSide+w;
			na=buff3D[i].normal;
			va=buff3D[i].vertex;
			v[0]=buff3D[i+1].vertex;
			v[1]=buff3D[i+areaSide].vertex;
			v[2]=buff3D[i-1].vertex;
			v[3]=buff3D[i-areaSide].vertex;
			getNormalVector(va,v[1],v[0],n[0]);
			getNormalVector(va,v[2],v[1],n[1]);
			getNormalVector(va,v[3],v[2],n[2]);
			getNormalVector(va,v[0],v[3],n[3]);
			na[0]=n[0][0]+n[1][0]+n[2][0]+n[3][0];
			na[1]=n[0][1]+n[1][1]+n[2][1]+n[3][1];
			na[2]=n[0][2]+n[1][2]+n[2][2]+n[3][2];
			getUnitVector(na);
		}
		// right side normals: w=areaSide-1
		//    1
		//  / |
		// 2 -a
		//  \ |
		//    3
		i=(h+1)*areaSide-1;
		na=buff3D[i].normal;
		va=buff3D[i].vertex;
		v[1]=buff3D[i+areaSide].vertex;
		v[2]=buff3D[i-1].vertex;
		v[3]=buff3D[i-areaSide].vertex;
		getNormalVector(va,v[2],v[1],n[1]);
		getNormalVector(va,v[3],v[2],n[2]);
		na[0]=n[1][0]+n[2][0];
		na[1]=n[1][1]+n[2][1];
		na[2]=n[1][2]+n[2][2];
		getUnitVector(na);
	}
	for(w=1;w<areaSide-1;w++)
	{
		// top side normals: h=areaSide-1,w>0 and w<areaSide-1
		// 
		//
		// 2 -a- 0
		//  \ | /
		//    3
		i=(areaSide-1)*areaSide+w;
		na=buff3D[i].normal;
		va=buff3D[i].vertex;
		v[0]=buff3D[i+1].vertex;
		v[2]=buff3D[i-1].vertex;
		v[3]=buff3D[i-areaSide].vertex;
		getNormalVector(va,v[3],v[2],n[2]);
		getNormalVector(va,v[0],v[3],n[3]);
		na[0]=n[2][0]+n[3][0];
		na[1]=n[2][1]+n[3][1];
		na[2]=n[2][2]+n[3][2];
		getUnitVector(na);
	}
	// bottom left normal: h=0,w=0
	//    1
	//    | \
	//    a- 0
	// 
	// 
	i=0;
	na=buff3D[i].normal;
	va=buff3D[i].vertex;
	v[0]=buff3D[i+1].vertex;
	v[1]=buff3D[i+areaSide].vertex;
	getNormalVector(va,v[1],v[0],na);
	// top left normal: h=areaSide-1,w=0
	//
	//
	//    a- 0
	//    | /
	//    3
	i=(areaSide-1)*areaSide;
	na=buff3D[i].normal;
	va=buff3D[i].vertex;
	v[0]=buff3D[i+1].vertex;
	v[3]=buff3D[i-areaSide].vertex;
	getNormalVector(va,v[0],v[3],na);
	// top right normal: h=areaSide-1,w=areaSide-1
	//
	//
	// 2 -a
	//  \ |
	//    3
	i=areaSide*areaSide-1;
	na=buff3D[i].normal;
	va=buff3D[i].vertex;
	v[2]=buff3D[i-1].vertex;
	v[3]=buff3D[i-areaSide].vertex;
	getNormalVector(va,v[3],v[2],na);
	// bottom right normal: h=0,w=areaSide-1
	//    1
	//  / |
	// 2 -a
	//
	//
	i=areaSide-1;
	na=buff3D[i].normal;
	va=buff3D[i].vertex;
	v[1]=buff3D[i+areaSide].vertex;
	v[2]=buff3D[i-1].vertex;
	getNormalVector(va,v[2],v[1],na);

	compile3D();
}
void Trilat::setIterParams(double minStep, int maxCount, double minGrad, double stepReduct0, double stepGrow0, double errDist)
{
	if(minGrad<=0)
		minGrad=DEF_MIN_ITER_GRAD;
	if(minStep<=0)
		minGrad=DEF_MIN_ITER_STEP;
	if(maxCount<=0)
		maxCount=DEF_MAX_ITER_COUNT;
	if(stepReduct0>1||stepReduct0<0)
	{
		if(stepGrow0>1)
			stepReduct0=INV(stepGrow0);
		else
			stepReduct0=DEF_STEP_REDUCT;
	}
	if(stepGrow0<1)
		stepGrow0=1.0;
	if(errDist<0)
		errDist=0;
	tbMinIterStep.setDouble(minStep);
	tbMaxIterCount.setInt(maxCount);
	tbMinIterGrad.setDouble(minGrad);
	tbStepReduct.setDouble(stepReduct0);
	tbStepGrow.setDouble(stepGrow0);
	tbErrDist.setDouble(errDist);
	minIterStep=minStep;
	maxStepMass=0;
	maxIterCount=maxCount;
	minIterGrad=minGrad;
	stepReduct=stepReduct0;
	stepGrow=stepGrow0;
	iterErrSquare=errDist*errDist;
}
void Trilat::setThreshold(double inf, double sup)
{
	if(inf>sup)
		inf=sup;
	thresSup=sup;
	thresInf=inf;
	tbInf.setDouble(inf);
	tbSup.setDouble(sup);
}
BOOL Trilat::setMapLims(double min, double max)
{
	if(min>max)
		return FALSE;
	mapmin=min;
	mapmax=max;
	showMapLims();
	return TRUE;
}
void Trilat::adjustGraph(	int gridx, int gridy,
							int precx, int precy,
							double margex, double margey,
							double scalex, double scaley,
							BOOL smooth, BOOL symAuto,
							BOOL log, BOOL exposant,
							BOOL toolbox)
{
	if(gridx<0)
		gridx=0;
	if(gridy<0)
		gridy=0;
	if(precx<0)
		precx=0;
	else if(precx>9)
		precx=9;
	if(precy<0)
		precy=0;
	else if(precy>9)
		precy=9;
	if(margex<0)
		margex=0;
	if(margey<0)
		margey=0;
	if(scalex<0)
		scalex=0;
	if(scaley<0)
		scaley=0;
	fg.graph.grid.set(gridx,gridy);
	fg.graph.formatX.precision=(char)precx;
	fg.graph.formatY.precision=(char)precy;
	fg.graph.formatY.exposant=exposant;
	fg.graph.marginChannel.set((float)margex,(float)margey);
	fg.graph.scale.set((float)scalex,(float)scaley);
	fg.graph.smooth=smooth;
	fg.graph.symAuto=symAuto;
	fg.graph.logscaleX=log;
	fg.graph.draw_to_index=TRUE;
	fg.toolbox=toolbox;
	fg.build();
}
void Trilat::showMapLims()
{
	tbMapMin.setDouble(mapmin);
	tbMapMax.setDouble(mapmax);
}
void Trilat::showPos()
{
	tbRx.setDouble(R.x);
	tbRy.setDouble(R.y);
	tbIterStartX.setDouble(p[POS_START].x);
	tbIterStartY.setDouble(p[POS_START].y);
}
void Trilat::swap(int beacon)
{
	StatBeacon *sb=stats+beacon;
	Point tmp=R;
	R=B[beacon];
	B[beacon]=tmp;
	sb->devx=estStat.stdev.x;
	sb->devy=estStat.stdev.y;
	sb->corxy=estStat.correl;
	if(beacon==active)
		updateActiveInfo();
	showPos();
	updateStat(beacon);
	prefetch();
}
BOOL Trilat::setActiveBeacon(int newActiveBeacon)
{
	if(newActiveBeacon<0||newActiveBeacon>=N||newActiveBeacon==active)
		return FALSE;
	active=chActive.setSelectedIndex(newActiveBeacon);
	updateActiveInfo();
	startMap();
	return TRUE;
}
void Trilat::reset()
{
	setImage(imgSide,areaSide);
	setArray(N,K,keepErrors);
	setModel(model);
	setStat(stdevXY,correlXY,stdevL,meanL,randev);
	setOptim(optim);
	//setRadius(radius);
	buildStat();
	updateStats();
	recalc();
}
void Trilat::setOrder(int newOrder)
{
	order=chOrder.setSelectedIndex(newOrder);
	orderStart=p[POS_START];
	orderEnd=R;
	locateBeacons();
	updateActiveInfo();
}
BOOL Trilat::locateBeacons()
{
	switch(order)
	{
		case ORDER_RAND:
		{
			double center=0.5*areaSide;
			for(int i=0;i<N;i++)
				B[i].set(center*(1+random(-1,1)),center*(1+random(-1,1)));
		}
		break;
		case ORDER_CIRCLE:
		{
			double radius=orderEnd.dist(orderStart);
			if(radius>0)
			{
				double tmp=2*PI*kBeacons;
				for(int i=0;i<N;i++)
					B[i].set(orderEnd.x+radius*cos(i*tmp),orderEnd.y+radius*sin(i*tmp));
			}
		}
		break;
		case ORDER_LINE:
		{
			double length=orderEnd.dist(orderStart);
			if(length>0)
			{
				double dl=length/(N-1),kl,rl=INV(length);
				for(int i=0;i<N;i++)
				{
					kl=1.0-(length-i*dl)*rl;
					B[i].set(orderStart.x+kl*(orderEnd.x-orderStart.x),orderStart.y+kl*(orderEnd.y-orderStart.y));
				}
			}
		}
		break;
		default:
			return FALSE;
	}
	return TRUE;
}
void Trilat::setMap(int newMap, int newArg)
{
	mapArg=chVar.setSelectedIndex(newArg);
	if(mapArg!=newArg)
	{
		mapArgChanged=TRUE;
		mapArg=newArg;
	}
	newMap=chMap.setSelectedIndex(newMap);
	if(map!=newMap)
	{
		mapChanged=!!map;
		map=newMap;
		mapType=MAP_TYPE_NONE;
		mapNoArg=FALSE;
		mapSection=TRUE;
		assert(map<MAP_COUNT);
		switch(map)
		{
			case MAP_ENERGY:
			case MAP_GRAD_ENERGY:
			case MAP_MLK:
				mapNoArg=TRUE;
				mapType=MAP_TYPE_ENERGY;
				break;
			case MAP_NUM_VAR:
			case MAP_NUM_VAR_X:
			case MAP_NUM_VAR_Y:
			case MAP_NUM_CORREL:
			case MAP_MEAN:
			case MAP_MEAN_X:
			case MAP_MEAN_Y:
			case MAP_EXCESS:
			case MAP_EXCESS_X:
			case MAP_EXCESS_Y:
			case MAP_SKEW:
			case MAP_SKEW_X:
			case MAP_SKEW_Y:
			case MAP_ITER_MISS:
				mapType=MAP_TYPE_NUM;
				break;
			case MAP_DIFF_VAR:
			case MAP_DIFF_CORREL:
				mapType=MAP_TYPE_DIFF;
				break;
			case MAP_CALC_ERROR:
			case MAP_TRANS_FACTOR:
			case MAP_TRANS_FULL:
			case MAP_EST_VAR:
			case MAP_EST_VAR_X:
			case MAP_EST_VAR_Y:
			case MAP_EST_CORREL:
			case MAP_PAR_VAR:
			case MAP_PAR_ERR:
			case MAP_MASS:
			case MAP_FULLMASS:
			case MAP_GRAD_MASS:
			case MAP_ITERS:
				mapType=MAP_TYPE_EST;
				break;
			case MAP_DISTRIB_EST:
			case MAP_DISTRIB_NUM:
			case MAP_DISTRIB_ERR:
				mapNoArg=TRUE;
				mapSection=FALSE;
				mapType=MAP_TYPE_DISTRIB;
				break;
		}
	}
}
double Trilat::getParabVar(double x, double y)
{
	double xc,yc;
	if(!parabOK)
		return 0;
	xc=x-p[POS_ERRMIN].x;
	yc=y-p[POS_ERRMIN].y;
	return minVar.x+minVar.y+xc*xc*U[0][0]+yc*yc*U[1][0]+xc*yc*U[2][0];
}
BOOL Trilat::getParabCoef(BOOL forced)
{
	int i;
	double xc,yc,covar,minCovar,x,y;
	Point out,var;

	parabOK=FALSE;
	detU=0;
	if(model==MODEL_S_TOF&&cog_ok)
	{
		if(processing&&!forced)
		{
			PRINT("Parab pending...");
			parabPending=TRUE;
		}
		else
		{
			getStatEst(p[POS_ERRMIN].x,p[POS_ERRMIN].y,out,minVar,minCovar,iterOut,FALSE);
			for(i=0;i<N;i++)
			{
				x=B[i].x;
				y=B[i].y;
				getStatEst(x,y,out,var,covar,iterOut,FALSE);
				V[i][0]=(var.x+var.y)-(minVar.y+minVar.x);
				xc=x-p[POS_ERRMIN].x;
				yc=y-p[POS_ERRMIN].y;
				F[i][0]=xc*xc;
				F[i][1]=yc*yc;
				F[i][2]=xc*yc;
			}
			if(invertF.inv(DBL_EPSILON))
			{
				mulMatrix(invF,V,U,3,N,1);
				detU=U[0][0]*U[1][0]-U[2][0]*U[2][0];
				parabOK=detU>DBL_EPSILON;
				recalc();
			}
		}
	}
	return parabOK;
}
BOOL Trilat::setSection(int newSectId, int lsmCount)
{
	if(lsmCount<0)
		lsmCount=0;
	if(!vSectCoefs.setSize(lsmCount))
		return FALSE;
	section=chSection.setSelectedIndex(newSectId);
	tbLSM.setInt(lsmCount);
	updateSection();
	return TRUE;
}
BOOL Trilat::setGraph(int newGraph, int newPoints)
{
	int i;
	if(newGraph<0||newGraph>=GRAPH_COUNT)
		return FALSE;
	if(newPoints<3)
		newPoints=3;
	{
		Lock lock(&fg.graph.cs);
		if(points!=newPoints)
		{
			if(!tGraphBuffers.setSize(CHNL_COUNT,newPoints))
				return FALSE;
			tGraphBuffers.clear();
			buff=tGraphBuffers.elems;
			for(i=0;i<CHNL_COUNT;i++)
				channels[i]->setSource(buff[i],-newPoints);
			points=newPoints;
			tbPoints.setInt(newPoints);
		}
	}
	if(graph!=newGraph)
	{
		graph=chGraph.setSelectedIndex(newGraph);
		for(i=0;i<CHNL_COUNT;i++)
		{
			channels[i]->readable=FALSE;
			//channels[i]->hidden=TRUE;
		}
		switch(newGraph)
		{
		case GRAPH_SECT:
			channels[CHNL_SECT_VAL]->readable=TRUE;
			channels[CHNL_SECT_LSM]->readable=TRUE;
			break;
		case GRAPH_DENS:
			for(i=CHNL_DENS_X_EST;i<CHNL_CUST_ARG;i++)
				channels[i]->readable=TRUE;
			break;
		case GRAPH_CUST:
			for(i=CHNL_CUST_ARG+1;i<CHNL_ITER_COUNT;i++)
				channels[i]->readable=TRUE;
			break;
		case GRAPH_ROUTE:
			for(i=CHNL_ITER_ROUTE;i<CHNL_COUNT;i++)
				channels[i]->readable=TRUE;
			break;
		}
		fg.build();
	}
	switch(newGraph)
	{
	case GRAPH_SECT:
		updateSection();
		break;
	case GRAPH_DENS:
	//case GRAPH_ROUTE:
		recalc();
		break;
	}
	return TRUE;
}
void Trilat::adjustWindow()
{
	float // the flat graph should not get too small:
		fw=max(window.width-imgRect.width-59.0f,MIN_GRAPH_WIDTH),
		fh=max(window.height-(float)imgRect.y-9.0f,MIN_GRAPH_HEIGHT);
	//PRINT(_T("fw:%.2f, fh:%.2f\n"),fw,fh);
	fg.size.set(fw,fh);
	imgRect.x=window.width-imgRect.width-20;
	tbWinSizeX.setInt(window.width);
	tbWinSizeY.setInt(window.height);
	container.build();
}
BOOL Trilat::setImage(int newImageSide, int newAreaSide)
{
	if(newAreaSide<10)
		newAreaSide=10;
	if(newImageSide<100)
		newImageSide=100;
	zoom=float(newImageSide)/newAreaSide;
	imgRect.width=(float)newImageSide;
	imgRect.height=(float)newImageSide;
	if(areaSide!=newAreaSide)
	{
		int newAreaSize=newAreaSide*newAreaSide;
		if(	!image.setSize(newAreaSide,newAreaSide)||
			!(levels=vLevels.setSize(newAreaSize))||
			!(buff3D=vBuff3D.setSize(newAreaSize)))
			goto ERROR_SET_ARRAY;
		custStart.set(0,0);
		custEnd.set(newAreaSide,newAreaSide);
		areaSide=newAreaSide;
		areaSize=newAreaSize;
		tbArea.setInt(newAreaSide);
	}
	tbImage.setInt(newImageSide);
	imgSide=newImageSide;

	adjustWindow();

	return TRUE;

ERROR_SET_ARRAY:
	reset();
	return FALSE;
}
void Trilat::updateActiveInfo()
{
	StatBeacon *sb;
	if(!stats||active<0||active>=N)
		return;
	sb=&stats[active];
	tbActiveX.setDouble(B[active].x);
	tbActiveY.setDouble(B[active].y);
	tbActiveStdevX.setDouble(sb->devx);
	tbActiveStdevY.setDouble(sb->devy);
	tbActiveStdevL.setDouble(sb->devl);
	tbActiveMeanL.setDouble(sb->meanl);
	tbActiveCorrel.setDouble(sb->corxy);
}
void Trilat::setActiveInfo(double x, double y, double devx, double devy, double corxy, double devl, double meanl)
{
	StatBeacon *sb;

	if(devx<0)
		devx=0;
	if(devy<0)
		devy=0;
	if(devl<0)
		devl=0;
	if(meanL<0)
		meanL=0;
	if(corxy<-1)
		corxy=-1;
	else if(corxy>1)
		corxy=1;
	sb=&stats[active];
	sb->devx=devx;
	sb->devy=devy;
	sb->devl=devl;
	sb->corxy=corxy;
	sb->meanl=meanl;
	B[active].set(x,y);
	updateStat(active);
	updateActiveInfo();
	updateOpt();
	prefetch();
}
void Trilat::setStat(	double newStdevXY,
						double newCorrelXY,
						double newStdevL,
						double newMeanL,
						BOOL newRandev)
{
	newCorrelXY=limit(newCorrelXY,-1.0,1.0);
	if(newStdevXY<0)
		newStdevXY=0;
	if(newStdevL<0)
		newStdevL=0;
	if(newMeanL<0)
		newMeanL=0;
	tbStdevXY.setDouble(newStdevXY);
	tbCorrXY.setDouble(newCorrelXY);
	tbStdevL.setDouble(newStdevL);
	tbMeanL.setDouble(newMeanL);
	bRandev.checked=newRandev;
	stdevXY=newStdevXY;
	correlXY=newCorrelXY;
	stdevL=newStdevL;
	meanL=newMeanL;
	randev=newRandev;
}
BOOL Trilat::setOptim(int newOptim)
{
	optim=chOpt.setSelectedIndex(newOptim);
	updateOpt();
	return TRUE;
}
void Trilat::buildStat()
{
	int i;
	StatBeacon *sb;
	for(i=0;i<N;i++)
	{
		sb=&stats[i];
		if(randev)
		{
			sb->devx=0.5*SQRT2*random(0,stdevXY);
			sb->devy=0.5*SQRT2*random(0,stdevXY);
			sb->devl=random(0,stdevL);
			sb->corxy=random(-correlXY,correlXY);
			sb->meanl=random(0,meanL);
		}
		else
		{
			sb->devx=0.5*SQRT2*stdevXY;
			sb->devy=0.5*SQRT2*stdevXY;
			sb->devl=stdevL;
			sb->corxy=correlXY;
			sb->meanl=meanL;
		}
	}
}
void Trilat::updateStat(int beacon, BOOL updateErrors)
{
	int i;
	double dx,dy;
	Error *error;
	MATRIX P;
	StatBeacon *sb;

	P=mP[beacon].elems;
	sb=stats+beacon;
	// covariance matrix:
	P[0][0]=sb->devx*sb->devx;
	P[1][1]=sb->devy*sb->devy;
	P[0][1]=P[1][0]=sb->corxy*sb->devx*sb->devy;
	P[2][2]=sb->devl*sb->devl;
	// error generation:
	if(updateErrors&&errors)
	{
		for(i=0;i<K;i++)
		{
			error=errors[i]+beacon;
			random_n(&dx,&dy,sb->corxy,sb->devx,sb->devy);
			error->x=(float)dx;
			error->y=(float)dy;
			error->l=(float)random_n(sb->devl);
		}
	}
}
void Trilat::updateStats(BOOL updateErrors)
{
	for(int i=0;i<N;i++)
		updateStat(i,updateErrors);
	updateOpt();
	updateActiveInfo();
	prefetch();
}
void Trilat::updateOpt()
{
	int i;
	double optsum=0,opti;

	for(i=0;i<N;i++)
	{
		switch(optim)
		{
		case OPT_DET:
			opti=mP[i].det();
			break;
		case OPT_TRACE:
			opti=mP[i].trace();
			break;
		default:
			opti=1.0;
		}
		optsum+=O[i]=INV(opti);
	}
	double den=INV(optsum);
	for(i=0;i<N;i++)
		O[i]*=den;
}
void Trilat::prefetch()
{
	double tmp1,tmp2,massi;
	const double *a1,*a2;
	int i,j;

	INVA_OK=FALSE;
	cog_ok=FALSE;
	FGW=0;
	vGradM.clear();
	maxGradM=0;
	for(i=0;i<N;i++)
		R2[i]=B[i].square();
	// Matrix A building
	mA.clear();
	switch(model)
	{
	case MODEL_TOF:
		for(i=0;i<rows;i++)
		{
			A[i][0]=B[0].x-B[i+1].x;
			A[i][1]=B[0].y-B[i+1].y;
		}
		break;
	case MODEL_S_TOF:
		for(i=0;i<rows-1;i++)
		{
			A[i][0]=B[i].x-B[i+1].x;
			A[i][1]=B[i].y-B[i+1].y;
		}
		A[rows-1][0]=B[rows-1].x-B[0].x;
		A[rows-1][1]=B[rows-1].y-B[0].y;
		break;
	case MODEL_TDOA:
		for(i=0;i<rows;i++)
		{
			A[i][0]=B[0].x-B[i+1].x;
			A[i][1]=B[0].y-B[i+1].y;
		}
		break;
	case MODEL_S_TDOA2:
		for(i=0;i<rows-1;i++)
		{
			A[i][0]=B[i].x-B[i+1].x;
			A[i][1]=B[i].y-B[i+1].y;
		}
		A[rows-1][0]=B[rows-1].x-B[0].x;
		A[rows-1][1]=B[rows-1].y-B[0].y;
		break;
	default:
		return;
	}
	if(!invMatrixA())
		return;
	
	mulMatrix(A,invA,Ea,rows,cols,rows); // Ea = AA*
	for(i=0;i<rows;i++)
		Ea[i][i]-=1;
	// A priori vM calculation (only TOF model)
	a1=invA[0];
	a2=invA[1];
	switch(model)
	{
	case MODEL_TOF:
		tmp1=a1[0];
		tmp2=a2[0];
		for(i=1;i<rows;i++)
		{
			tmp1+=a1[i];
			tmp2+=a2[i];
		}
		massi=tmp1*tmp1+tmp2*tmp2;
		break;
	case MODEL_S_TOF:
		massi=(a1[0]-a1[N-1])*(a1[0]-a1[N-1])+(a2[0]-a2[N-1])*(a2[0]-a2[N-1]); // mass_0
		if(motionEstim)
		{
			Point *gm=gradM;
			tmp1=(a1[N-1]-a1[0])*(a1[N-1]-a1[0])+(a2[N-1]-a2[0])*(a2[N-1]-a2[0]); // omega_00
			tmp2=Ea[0][0]-Ea[0][N-1]-Ea[N-1][0]+Ea[N-1][N-1];// epselon_00
			gm->x=(tmp1+tmp2*Da[0][0])*(a1[N-1]-a1[0])+tmp2*Da[0][1]*(a2[N-1]-a2[0]);
			gm->y=tmp2*Da[0][1]*(a1[N-1]-a1[0])+(tmp1+tmp2*Da[1][1])*(a2[N-1]-a2[0]);
			for(j=1;j<N;j++)
			{
				tmp1=(a1[j-1]-a1[j])*(a1[N-1]-a1[0])+(a2[j-1]-a2[j])*(a2[N-1]-a2[0]); // omega_j0
				tmp2=Ea[j][0]-Ea[j][N-1]-Ea[j-1][0]+Ea[j-1][N-1]; // epselon_j0
				gm->x+=(tmp1+tmp2*Da[0][0])*(a1[j-1]-a1[j])+tmp2*Da[0][1]*(a2[j-1]-a2[j]);
				gm->y+=tmp2*Da[0][1]*(a1[j-1]-a1[j])+(tmp1+tmp2*Da[1][1])*(a2[j-1]-a2[j]);
			}
			gm->x*=2;
			gm->y*=2;
			maxGradM=gm->dist();
		}
		break;
	default:
		return;
	}
	FGW=M[0]=massi;
	for(i=1;i<N;i++)
	{
		switch(model)
		{
		case MODEL_TOF:
			massi=a1[i-1]*a1[i-1]+a2[i-1]*a2[i-1];
			break;
		case MODEL_S_TOF:
			massi=(a1[i]-a1[i-1])*(a1[i]-a1[i-1])+(a2[i]-a2[i-1])*(a2[i]-a2[i-1]); // mass_i
			if(motionEstim)
			{
				Point *gm=gradM+i;
				tmp1=(a1[N-1]-a1[0])*(a1[i-1]-a1[i])+(a2[N-1]-a2[0])*(a2[i-1]-a2[i]); //omega_0i
				tmp2=Ea[0][i]-Ea[0][i-1]-Ea[N-1][i]+Ea[N-1][i-1];// epselon_0i
				gm->x=(tmp1+tmp2*Da[0][0])*(a1[N-1]-a1[0])+tmp2*Da[0][1]*(a2[N-1]-a2[0]);
				gm->y=tmp2*Da[0][1]*(a1[N-1]-a1[0])+(tmp1+tmp2*Da[1][1])*(a2[N-1]-a2[0]);
				for(j=1;j<N;j++)
				{
					tmp1=(a1[j-1]-a1[j])*(a1[i-1]-a1[i])+(a2[j-1]-a2[j])*(a2[i-1]-a2[i]); //omega_ji
					tmp2=Ea[j][i]-Ea[j][i-1]-Ea[j-1][i]+Ea[j-1][i-1];// epselon_ji
					gm->x+=(tmp1+tmp2*Da[0][0])*(a1[j-1]-a1[j])+tmp2*Da[0][1]*(a2[j-1]-a2[j]);
					gm->y+=tmp2*Da[0][1]*(a1[j-1]-a1[j])+(tmp1+tmp2*Da[1][1])*(a2[j-1]-a2[j]);
				}
				gm->x*=2;
				gm->y*=2;
				tmp2=gm->dist();
				if(maxGradM<tmp2)
					maxGradM=tmp2;
			}
			break;
		}
		FGW+=M[i]=massi;
	}
	getCOG();
}
void Trilat::getCOG()
{
	double
		sumx,sumy,sumxx,sumyy,sumxy,
		det,massi,varx,vary,covar;
	CMATRIX P;
	Point *Ri;

	if(!FGW)
		return;
	sumx=sumy=sumxx=sumyy=sumxy=0;
	for(int i=0;i<N;i++)
	{
		P=mP[i].elems;
		Ri=B+i;
		massi=M[i];
		varx=P[0][0]+P[2][2];
		vary=P[1][1]+P[2][2];
		covar=P[1][0];
		sumx+=massi*(varx*Ri->x+covar*Ri->y);
		sumy+=massi*(covar*Ri->x+vary*Ri->y);
		sumxx+=massi*varx;
		sumyy+=massi*vary;
		sumxy+=massi*covar;
	}
	det=sumxx*sumyy-sumxy*sumxy;
	p[POS_ERRMIN].x=( sumyy*sumx-sumxy*sumy)/det;
	p[POS_ERRMIN].y=(-sumxy*sumx+sumxx*sumy)/det;

	cog_ok=TRUE;
	if(parabEstim)
		getParabCoef();
}
inline static double getCorrel(double covar, double stdev_product)
{
	if(!covar||!stdev_product)
		return 0;
	return covar/stdev_product;
}
void Trilat::recalc(BOOL custOK)
{
	int i;
	Limit<double> limX,limY,limL;
	double	x,y,li,dx,dy,dl,
			spanx,spany,spanl,minx,miny,minl;
	StatBeacon *sb=stats+active;
	PrecisionTimer timer;

	getStatNum(R.x,R.y,numStat,TRUE);
	getStatEst(R.x,R.y,estR,estVar,estCovar,iterOut,TRUE);
	estStat.stdev.x=sqrt(estVar.x);
	estStat.stdev.y=sqrt(estVar.y);
	estStat.correl=getCorrel(estCovar,estStat.stdev.x*estStat.stdev.y);
	dx=R.x-B[active].x;
	dy=R.y-B[active].y;
	estStat.stdevL=sqrt((pow2(dx*sb->devx)+pow2(dy*sb->devy))/(dx*dx+dy*dy)+pow2(sb->devl));
	if(graph==GRAPH_DENS)
	{
		Error *distrib;
		for(i=0;i<K;i++)
		{
			distrib=distribs+i;
			limX.add(distrib->x);
			limY.add(distrib->y);
			limL.add(distrib->l);
		}
		// expand span
		limX.change(2);
		limY.change(2);
		limL.change(2);
		minx=limX.minValue;
		miny=limY.minValue;
		minl=limL.minValue;
		spanx=limX.span();
		spany=limY.span();
		spanl=limL.span();
		if(spanx&&spany&&spanl)
		{
			// get steps
			double tmp=INV(points-1);
			dx=spanx*tmp;
			dy=spany*tmp;
			dl=spanl*tmp;
			for(i=0;i<points;i++)
			{
				buff[CHNL_DENS_X_ARG][i]=x=minx+i*dx;
				buff[CHNL_DENS_Y_ARG][i]=y=miny+i*dy;
				buff[CHNL_DENS_L_ARG][i]=li=minl+i*dl;
				buff[CHNL_DENS_X_EST][i]=normalDensity(x,0,estStat.stdev.x);
				buff[CHNL_DENS_Y_EST][i]=normalDensity(y,0,estStat.stdev.y);
				buff[CHNL_DENS_L_EST][i]=normalDensity(li,0,estStat.stdevL);
			}
			// get particle sizes
			dx=INV(K*dx);
			dy=INV(K*dy);
			dl=INV(K*dl);
			// get factors
			spanx=points/spanx;
			spany=points/spany;
			spanl=points/spanl;
			// clear accums
			ClearArray(buff[CHNL_DENS_X_NUM],points);
			ClearArray(buff[CHNL_DENS_Y_NUM],points);
			ClearArray(buff[CHNL_DENS_L_NUM],points);
			Error *distrib;
			for(i=0;i<K;i++)
			{
				distrib=distribs+i;
				buff[CHNL_DENS_X_NUM][__int(spanx*(distrib->x-minx))]+=dx;
				buff[CHNL_DENS_Y_NUM][__int(spany*(distrib->y-miny))]+=dy;
				buff[CHNL_DENS_L_NUM][__int(spanl*(distrib->l-minl))]+=dl;
			}
			// make diffs
			for(i=0;i<points;i++)
			{
				dx=buff[CHNL_DENS_X_NUM][i]-buff[CHNL_DENS_X_EST][i];
				dy=buff[CHNL_DENS_Y_NUM][i]-buff[CHNL_DENS_Y_EST][i];
				dl=buff[CHNL_DENS_L_NUM][i]-buff[CHNL_DENS_L_EST][i];
				buff[CHNL_DENS_X_DIF][i]=dx;
				buff[CHNL_DENS_Y_DIF][i]=dy;
				buff[CHNL_DENS_L_DIF][i]=dl;
			}
			fg.graph.update();
		}
	}
	recalc_time=timer.time();
	if(custOK)
	{
		addCust();
		if(graph==GRAPH_CUST)
			fg.graph.update();
	}
}

#define ADD_QUAD_ENERGY(q,i) quad[q][2]+=pow2(B[i].dist(quad[q][0],quad[q][1])-L[i])*O[i]
BOOL Trilat::getStatNum(double x, double y, FullStat &stat, BOOL calcDistrib)
{
	int i,j,k,iterErrCount=0;
	double	tmpx=0,tmpy=0,tmpl=0,
			tmpx2=0,tmpy2=0,tmpxy=0,tmpl2=0,
			tmpx3=0,tmpy3=0,tmpl3=0,
			tmpx4=0,tmpy4=0,tmpl4=0,
			dx=0,dy=0,dl,la=B[active].dist(x,y),
			outx=0,outy=0,
			mx,my,ml,sx,sy,sl,
			error_x,error_y,
			*L=Le,value;
	Error	*error_samples,*error,*distrib;
	StatBeacon *sb;

	if(!K)
	{
		ClearStruct(stat);
		return FALSE;
	}

	for(k=0,statProcess=TRUE;k<K&&statProcess;k++)
	{
		error_samples=errors?errors[k]:NULL;
		for(i=0;i<N;i++)
		{
			sb=stats+i;
			if(error_samples)
			{
				error=error_samples+i;
				value=getLength(B[i].x-x+error->x,B[i].y-y+error->y)+error->l;
			}
			else
			{
				random_n(&error_x,&error_y,sb->corxy,sb->devx,sb->devy);
				value=getLength(B[i].x-x+error_x,B[i].y-y+error_y)+random_n(sb->devl);
			}
			L[i]=sb->meanl+value;
		}
		if(model<MODEL_ITER)
		{
			switch(model)
			{
				case MODEL_TOF:
				{
					double tmp=R2[0]-L[0]*L[0];
					for(i=0;i<rows;i++)
						D[i][0]=0.5*(L[i+1]*L[i+1]-R2[i+1]+tmp);
				}
				break;
				case MODEL_S_TOF:
				{
					for(i=0;i<rows-1;i++)
						D[i][0]=0.5*((R2[i]-L[i]*L[i])-(R2[i+1]-L[i+1]*L[i+1]));
					D[rows-1][0]=0.5*((R2[rows-1]-L[rows-1]*L[rows-1])-(R2[0]-L[0]*L[0]));
				}
				break;
				case MODEL_TDOA:
				{
					double tmp,l1=L[0];
					for(i=0;i<rows;i++)
					{
						tmp=l1-L[i+1];
						A[i][2]=tmp;
						D[i][0]=0.5*(R2[0]-R2[i+1]+tmp*tmp);
					}
					if(!invMatrixA())
						return FALSE;
				}
				break;
				case MODEL_S_TDOA:
				{
					double
						tmp1=L[rows-1]-L[0],tmp2=L[0]-L[1];
					A[0][0]=(B[0].x-B[1].x)*tmp1-(B[rows-1].x-B[0].x)*tmp2;
					A[0][1]=(B[0].y-B[1].y)*tmp1-(B[rows-1].y-B[0].y)*tmp2;
					D[0][0]=0.5*((R2[0]-R2[1]+tmp2*tmp2)*tmp1-(R2[rows-1]-R2[0]-tmp1*tmp1)*tmp2);
					for(i=1;i<rows-1;i++)
					{
						tmp1=tmp2;
						tmp2=L[i]-L[i+1];
						A[i][0]=	(B[i].x-B[i+1].x)*tmp1-(B[i-1].x-B[i].x)*tmp2;
						A[i][1]=	(B[i].y-B[i+1].y)*tmp1-(B[i-1].y-B[i].y)*tmp2;
						D[i][0]=0.5*((R2[i]-R2[i+1]+tmp2*tmp2)*tmp1-(R2[i-1]-R2[i]-tmp1*tmp1)*tmp2);
					}
					tmp1=tmp2;
					tmp2=L[rows-1]-L[0];
					A[rows-1][0]=(B[rows-1].x-B[0].x)*tmp1-(B[rows-2].x-B[rows-1].x)*tmp2;
					A[rows-1][1]=(B[rows-1].y-B[0].y)*tmp1-(B[rows-2].y-B[rows-1].y)*tmp2;
					D[rows-1][0]=0.5*((R2[rows-1]-R2[0]+tmp2*tmp2)*tmp1-(R2[rows-2]-R2[rows-1]-tmp1*tmp1)*tmp2);
					if(!invMatrixA())
						return FALSE;
				}
				break;
				case MODEL_S_TDOA2:
				{
					for(i=0;i<rows-1;i++)
					{
						D[i][0]=0.5*(R2[i]-L[i]*L[i]-R2[i+1]+L[i+1]*L[i+1]);
						A[i][2]=L[i]-L[i+1];
					}
					D[rows-1][0]=0.5*(R2[rows-1]-L[rows-1]*L[rows-1]-R2[0]+L[0]*L[0]);
					A[rows-1][2]=L[rows-1]-L[0];
					// MATH:
					if(!invMatrixA())
						return FALSE;
				}
				break;
				default:
					assert(FALSE);
			}
			outx=invA[0][0]*D[0][0];
			outy=invA[1][0]*D[0][0];
			for(i=1;i<rows;i++)
			{
				outx+=invA[0][i]*D[i][0];
				outy+=invA[1][i]*D[i][0];
			}
			dx=outx-x;
			dy=outy-y;
		}
		else
		{
			BOOL enhanced=FALSE;
			double step, estR[2], estRprev[2];
			estRprev[0]=estR[0]=p[POS_START].x;
			estRprev[1]=estR[1]=p[POS_START].y;

			switch(model)
			{
				case MODEL_ITER_A2:
					enhanced=TRUE;
				case MODEL_ITER_A:
				{
					double
						Ox,Oy,rO,
						tmpR[2],estP[2][2],estRi[2],
						invP[2][2],estPi[2][2],invPi[2][2],**P,
						li,deti,detr,Po[2][2],Jo[2][2],Psum[2][2];

					ClearMemory(estP,sizeof(estP));
					for(j=0;j<maxIterCount;j++)
					{
						if(optim)
						{
							ClearMemory(invP,sizeof(invP));
							ClearMemory(tmpR,sizeof(tmpR));
							for(i=0;i<N;i++)
							{
								li=L[i];
								P=mP[i].elems;
								// Oi = (Rk-1 - Ri) / ||Rk-1 - Ri||
								Ox=estRprev[0]-B[i].x;
								Oy=estRprev[1]-B[i].y;
								rO=INV(getLength(Ox,Oy));
								Ox*=rO;
								Oy*=rO;
								// estRi = vB + li * Oi
								estRi[0]=B[i].x+li*Ox;
								estRi[1]=B[i].y+li*Oy;
								if(enhanced)
								{
									// P2 = vi*Psum*vi
									Jo[0][0]=				(1-Ox*Ox)*rO;
									Jo[0][1]=Jo[1][0]=		-(Ox*Oy)*rO;
									Jo[1][1]=				(1-Oy*Oy)*rO;
									Psum[0][0]=				estP[0][0]+P[0][0];
									Psum[0][1]=Psum[1][0]=	estP[0][1]+P[0][1];
									Psum[1][1]=				estP[1][1]+P[1][1];
									mulMatrixTrans(Jo,Psum,Po);
									// P
									estPi[0][0]=			P[0][0]+P[2][2]*Ox*Ox+(li*li+P[2][2])*Po[0][0];
									estPi[0][1]=estPi[1][0]=P[0][1]+P[2][2]*Ox*Oy+(li*li+P[2][2])*Po[0][1];
									estPi[1][1]=			P[1][1]+P[2][2]*Oy*Oy+(li*li+P[2][2])*Po[1][1];
								}
								else
								{
									// P = bPi + VarLi * Oi * Oi'
									estPi[0][0]=			P[0][0]+P[2][2]*Ox*Ox;
									estPi[0][1]=estPi[1][0]=P[0][1]+P[2][2]*Ox*Oy;
									estPi[1][1]=			P[1][1]+P[2][2]*Oy*Oy;
								}
								// det(P)
								deti=estPi[0][0]*estPi[1][1]-estPi[0][1]*estPi[1][0];
								if(!deti)
									return FALSE;
								detr=INV(deti);
								// inv(P)
								invPi[0][0]=			 estPi[1][1]*detr;
								invPi[0][1]=invPi[1][0]=-estPi[0][1]*detr;
								invPi[1][1]=			 estPi[0][0]*detr;
								// estP^-1 = Sum (P^-1)
								invP[0][0]+=			invPi[0][0];
								invP[0][1]=invP[1][0]+=	invPi[0][1];
								invP[1][1]+=			invPi[1][1];
								// tmpR = Sum (P^-1 * estRi)
								tmpR[0]+=invPi[0][0]*estRi[0]+invPi[0][1]*estRi[1];
								tmpR[1]+=invPi[1][0]*estRi[0]+invPi[1][1]*estRi[1];
							}
							invMatrix(invP,estP);
							// estR = estP * tmpR
							estR[0]=estP[0][0]*tmpR[0]+estP[0][1]*tmpR[1];
							estR[1]=estP[1][0]*tmpR[0]+estP[1][1]*tmpR[1];
						}
						else
						{
							ClearMemory(estR,sizeof(estR));
							for(i=0;i<N;i++)
							{
								// Oi = (Rk-1 - Ri) / ||Rk-1 - Ri||
								Ox=estRprev[0]-B[i].x;
								Oy=estRprev[1]-B[i].y;
								rO=INV(getLength(Ox,Oy));
								Ox*=rO;
								Oy*=rO;
								// estRi = B + L * Oi
								estR[0]+=B[i].x+L[i]*Ox;
								estR[1]+=B[i].y+L[i]*Oy;
							}
							estR[0]*=kBeacons;
							estR[1]*=kBeacons;
						}
						step=getLength(estR[0]-estRprev[0],estR[1]-estRprev[1]);
						if(step<minIterStep)
							break;
						estRprev[0]=estR[0];
						estRprev[1]=estR[1];
					}
					break;
				}
				case MODEL_ITER_G:
				{
					double	Jg[2][2],invJg[2][2],Jxx,Jyy,Jxy,lx,ly,li,e,dex,dey,
							di,opti,lidi,lxdi,lydi;
					
					for(j=0;j<maxIterCount;j++)
					{
						for(i=0,dex=0,dey=0,Jxx=0,Jyy=0,Jxy=0,e=0;i<N;i++)
						{
							li=L[i];
							opti=O[i];
							lx=estR[0]-B[i].x;
							ly=estR[1]-B[i].y;
							di=getLength(lx,ly);
							e+=pow2(di-li)*opti;
							double coef=INV(di);
							lidi=coef*li;
							lxdi=coef*lx;
							lydi=coef*ly;
							dex+=opti*(1.0-lidi)*lx;
							dey+=opti*(1.0-lidi)*ly;
							Jxx+=opti*(1.0-lidi*(1-pow2(lxdi)));
							Jyy+=opti*(1.0-lidi*(1-pow2(lydi)));
							Jxy+=opti*lidi*lxdi*lydi;
						}
						Jg[0][0]=Jxx;
						Jg[1][1]=Jyy;
						Jg[0][1]=Jg[1][0]=Jxy;
						invMatrix(Jg,invJg);
						lx=-(invJg[0][0]*dex+invJg[0][1]*dey);
						ly=-(invJg[1][0]*dex+invJg[1][1]*dey);
						estR[0]+=lx;
						estR[1]+=ly;
						step=getLength(lx,ly);
						if(step<minIterStep)
							break;
					}
					break;
				}
				case MODEL_ITER_Q:
				{
					double quad[5][3],minE,cx,cy;
					int minQ,q;
					
					step=areaSide;
					quad[0][0]=estR[0];
					quad[0][1]=estR[1];
					quad[0][2]=0;
					for(i=0;i<N;i++)
						ADD_QUAD_ENERGY(0,i);
					minE=quad[0][2];
					minQ=0;
					for(j=0;j<maxIterCount;j++)
					{
						step*=(minQ==0)?0.5:1.1;

						// buildQuad
						cx=quad[minQ][0];
						cy=quad[minQ][1];
						// center
						quad[0][0]=cx;
						quad[0][1]=cy;
						// left top
						quad[1][0]=cx-step;
						quad[1][1]=cy-step;
						// right top
						quad[2][0]=cx+step;
						quad[2][1]=cy-step;
						// left bottom
						quad[3][0]=cx-step;
						quad[3][1]=cy+step;
						// right bottom
						quad[4][0]=cx+step;
						quad[4][1]=cy+step;
						switch(minQ)
						{
						case 1:
							quad[3][2]=quad[0][2];
							quad[1][2]=quad[2][2]=quad[4][2]=0;
							for(i=0;i<N;i++)
							{
								ADD_QUAD_ENERGY(1,i);
								ADD_QUAD_ENERGY(2,i);
								ADD_QUAD_ENERGY(4,i);
							}
							break;
						case 2:
							quad[4][2]=quad[0][2];
							quad[1][2]=quad[2][2]=quad[3][2]=0;
							for(i=0;i<N;i++)
							{
								ADD_QUAD_ENERGY(1,i);
								ADD_QUAD_ENERGY(2,i);
								ADD_QUAD_ENERGY(3,i);
							}
							break;
						case 3:
							quad[1][2]=quad[0][2];
							quad[2][2]=quad[3][2]=quad[4][2]=0;
							for(i=0;i<N;i++)
							{
								ADD_QUAD_ENERGY(2,i);
								ADD_QUAD_ENERGY(3,i);
								ADD_QUAD_ENERGY(4,i);
							}
							break;
						case 4:
							quad[2][2]=quad[0][2];
							quad[1][2]=quad[3][2]=quad[4][2]=0;
							for(i=0;i<N;i++)
							{
								ADD_QUAD_ENERGY(1,i);
								ADD_QUAD_ENERGY(3,i);
								ADD_QUAD_ENERGY(4,i);
							}
							break;
						default:
							quad[1][2]=quad[2][2]=quad[3][2]=quad[4][2]=0;
							for(i=0;i<N;i++)
							{
								ADD_QUAD_ENERGY(1,i);
								ADD_QUAD_ENERGY(2,i);
								ADD_QUAD_ENERGY(3,i);
								ADD_QUAD_ENERGY(4,i);
							}
							break;
						}
						minQ=0;
						quad[0][2]=minE;
						for(q=1;q<5;q++)
							if(quad[q][2]<quad[minQ][2])
								minQ=q;
						minE=quad[minQ][2];
						if(step<minIterStep)
							break;
					}
					estR[0]=quad[minQ][0];
					estR[1]=quad[minQ][1];
					break;
				}
				default:
					assert(FALSE);
			}
			outx=estR[0];
			outy=estR[1];
			dx=outx-x;
			dy=outy-y;
			if(iterErrSquare&&getSquare(dx,dy)>iterErrSquare)
				iterErrCount++;
		}
		tmpx+=dx;
		tmpy+=dy;
		tmpl+=dl=L[active]-la; // error in distance to active beacon
		tmpxy+=dx*dy;
		tmpx2+=dx*dx;
		tmpy2+=dy*dy;
		tmpl2+=dl*dl;
		tmpx3+=dx*dx*dx;
		tmpy3+=dy*dy*dy;
		tmpl3+=dl*dl*dl;
		tmpx4+=dx*dx*dx*dx;
		tmpy4+=dy*dy*dy*dy;
		tmpl4+=dl*dl*dl*dl;
		if(calcDistrib)
		{
			distrib=distribs+k;
			distrib->x=(float)dx;
			distrib->y=(float)dy;
			distrib->l=(float)dl;
		}
	}

	ClearStruct(stat);
	stat.mean.x=mx=kSample*tmpx;
	stat.mean.y=my=kSample*tmpy;
	stat.stdev.x=sx=sqrt(kSample*tmpx2-mx*mx);
	stat.stdev.y=sy=sqrt(kSample*tmpy2-my*my);
	if(sx&&sy)
	{
		stat.correl=(kSample*tmpxy-mx*my)/(sx*sy);
		stat.skew.x=(kSample*tmpx3-3*mx*kSample*tmpx2+2*mx*mx*mx)/(sx*sx*sx);
		stat.skew.y=(kSample*tmpy3-3*my*kSample*tmpy2+2*my*my*my)/(sy*sy*sy);
		stat.excess.x=(kSample*tmpx4-4*mx*kSample*tmpx3+6*mx*mx*kSample*tmpx2-3*mx*mx*mx*mx)/(sx*sx*sx*sx)-3;
		stat.excess.y=(kSample*tmpy4-4*my*kSample*tmpy3+6*my*my*kSample*tmpy2-3*my*my*my*my)/(sy*sy*sy*sy)-3;
	}
	
	stat.meanL=ml=kSample*tmpl;
	stat.stdevL=sl=sqrt(kSample*tmpl2-ml*ml);
	if(sl)
	{
		stat.skewL=(kSample*tmpl3-3*ml*kSample*tmpl2+2*ml*ml*ml)/(sl*sl*sl);
		stat.excessL=(kSample*tmpl4-4*ml*kSample*tmpl3+6*ml*ml*kSample*tmpl2-3*ml*ml*ml*ml)/(sl*sl*sl*sl)-3;
	}

	iterErrRatio=kSample*iterErrCount;

	return TRUE;
}
#define SQUARE_ELEMS(J) (J[0][0]*J[0][0]+J[0][1]*J[0][1]+J[0][2]*J[0][2]+J[1][0]*J[1][0]+J[1][1]*J[1][1]+J[1][2]*J[1][2])
BOOL Trilat::getStatEst(double x, double y, Point &out, Point &var, double &covar, IterInfo &iterOut, BOOL routing)
{
	int i,j=0;
	MATRIX J;
	double a1,a2,tmp,tmp1,tmp2,l1,lx,ly,li=0,e=0,dex=0,dey=0,di=0,*L=Lr;
	Point *start=p+POS_START;

	for(i=0,e=0,dex=0,dey=0;i<N;i++)
	{
		li=L[i]=B[i].dist(x,y);
		if(!start->equals(x,y))
		{
			di=start->dist(B[i]);
			e+=pow2(di-li)*O[i];
			double coef=(1-li/di)*O[i];
			dex+=coef*(B[i].x-x);
			dey+=coef*(B[i].y-y);
		}
	}
	iterOut.func=e;
	iterOut.grad.x=2*dex;
	iterOut.grad.y=2*dey;
	if(model<MODEL_ITER)
	{
		switch(model)
		{
			case MODEL_TOF:
			{
				// MATH:
				tmp=R2[0]-L[0]*L[0];
				double d,outx=0,outy=0;
				for(i=0;i<rows;i++)
				{
					d=0.5*(L[i+1]*L[i+1]-R2[i+1]+tmp);
					outx+=invA[0][i]*d;
					outy+=invA[1][i]*d;
				}
				out.x=outx;
				out.y=outy;
				// JACOBIENS:
				J=mJ[0].elems;
				for(a1=0,a2=0,i=0;i<rows;i++)
				{
					a1-=invA[0][i];
					a2-=invA[1][i];
				}
				lx=outx-B[0].x;
				ly=outy-B[0].y;
				li=L[0];
				J[0][0]=lx*a1;	J[0][1]=ly*a1;	J[0][2]=li*a1;
				J[1][0]=lx*a2;	J[1][1]=ly*a2;	J[1][2]=li*a2;
				mulMatrixTrans(J,mP[0],Px,2,3);
				for(i=1;i<N;i++)
				{
					J=mJ[i].elems;
					a1=invA[0][i-1];
					a2=invA[1][i-1];
					lx=outx-B[i].x;
					ly=outy-B[i].y;
					li=L[i];
					J[0][0]=lx*a1;	J[0][1]=ly*a1;	J[0][2]=li*a1;
					J[1][0]=lx*a2;	J[1][1]=ly*a2;	J[1][2]=li*a2;
					mulMatrixTrans(J,mP[i],Ptmp,2,3);
					addMatrix(Ptmp,Px,Px,2,2);
				}
			}
			break;
			case MODEL_S_TOF:
			{
				// MATH:
				double d=0.5*((R2[0]-L[0]*L[0])-(R2[1]-L[1]*L[1]));
				double outx=invA[0][0]*d;
				double outy=invA[1][0]*d;
				for(i=1;i<rows-1;i++)
				{
					d=0.5*((R2[i]-L[i]*L[i])-(R2[i+1]-L[i+1]*L[i+1]));
					outx+=invA[0][i]*d;
					outy+=invA[1][i]*d;
				}
				d=0.5*((R2[rows-1]-L[rows-1]*L[rows-1])-(R2[0]-L[0]*L[0]));
				outx+=invA[0][rows-1]*d;
				outy+=invA[1][rows-1]*d;
				out.x=outx;
				out.y=outy;
				// JACOBIENS:
				J=mJ[0].elems;
				a1=invA[0][N-1]-invA[0][0];
				a2=invA[1][N-1]-invA[1][0];
				lx=outx-B[0].x;
				ly=outy-B[0].y;
				li=L[0];
				J[0][0]=lx*a1;	J[0][1]=ly*a1;	J[0][2]=li*a1;
				J[1][0]=lx*a2;	J[1][1]=ly*a2;	J[1][2]=li*a2;
				mulMatrixTrans(J,mP[0],Px,2,3);
				for(i=1;i<N;i++)
				{
					J=mJ[i].elems;
					a1=invA[0][i-1]-invA[0][i];
					a2=invA[1][i-1]-invA[1][i];
					lx=outx-B[i].x;
					ly=outy-B[i].y;
					li=L[i];
					J[0][0]=lx*a1;	J[0][1]=ly*a1;	J[0][2]=li*a1;
					J[1][0]=lx*a2;	J[1][1]=ly*a2;	J[1][2]=li*a2;
					mulMatrixTrans(J,mP[i],Ptmp,2,3);
					addMatrix(Ptmp,Px,Px,2,2);
				}
			}
			break;
			case MODEL_TDOA:
			{
				// FILL:
				for(i=0;i<rows;i++)
				{
					tmp=L[0]-L[i+1];
					A[i][2]=tmp;
					D[i][0]=0.5*(R2[0]-R2[i+1]+tmp*tmp);
				}
				// MATH:
				if(!invMatrixA())
					return FALSE;
				mulMatrix(invA,D,X,cols,rows,1); // X = Ainv* D
				out.x=X[0][0];
				out.y=X[1][0];
				// JACOBIENS:
				l1=X[2][0];
				J=mJ[0].elems;
				for(a1=0,a2=0,i=0,tmp1=0,tmp2=0;i<rows;i++)
				{
					a1+=invA[0][i];
					a2+=invA[1][i];
					li=l1-A[i][2];
					tmp1+=li*invA[0][i];
					tmp2+=li*invA[1][i];
				}
				lx=out.x-B[0].x;
				ly=out.y-B[0].y;
				J[0][0]=lx*a1;	J[0][1]=ly*a1;	J[0][2]=-tmp1;
				J[1][0]=lx*a2;	J[1][1]=ly*a2;	J[1][2]=-tmp2;
				mulMatrixTrans(J,mP[0],Px,2,3);
				M[0]=SQUARE_ELEMS(J)/(2*li*li);
				for(i=1;i<N;i++)
				{
					J=mJ[i].elems;
					a1=-invA[0][i-1];
					a2=-invA[1][i-1];
					li=l1-A[i-1][2];
					lx=out.x-B[i].x;
					ly=out.y-B[i].y;
					J[0][0]=lx*a1;	J[0][1]=ly*a1;	J[0][2]=-li*a1;
					J[1][0]=lx*a2;	J[1][1]=ly*a2;	J[1][2]=-li*a2;
					mulMatrixTrans(J,mP[i],Ptmp,2,3);
					addMatrix(Ptmp,Px,Px,2,2);
					M[i]=SQUARE_ELEMS(J)/(2*li*li);
				}
			}
			break;
			case MODEL_S_TDOA:
			{
				// FILL:
				tmp1=L[N-1]-L[0];
				tmp2=L[0]-L[1];
				A[0][0]=(B[0].x-B[1].x)*tmp1-(B[N-1].x-B[0].x)*tmp2;
				A[0][1]=(B[0].y-B[1].y)*tmp1-(B[N-1].y-B[0].y)*tmp2;
				D[0][0]=0.5*((R2[0]-R2[1]+tmp2*tmp2)*tmp1-(R2[N-1]-R2[0]-tmp1*tmp1)*tmp2);
				for(i=1;i<N-1;i++)
				{
					tmp1=tmp2;
					tmp2=L[i]-L[i+1];
					A[i][0]=(B[i].x-B[i+1].x)*tmp1-(B[i-1].x-B[i].x)*tmp2;
					A[i][1]=(B[i].y-B[i+1].y)*tmp1-(B[i-1].y-B[i].y)*tmp2;
					D[i][0]=0.5*((R2[i]-R2[i+1]+tmp2*tmp2)*tmp1-(R2[i-1]-R2[i]-tmp1*tmp1)*tmp2);
				}
				tmp1=tmp2;
				tmp2=L[N-1]-L[0];
				A[N-1][0]=(B[N-1].x-B[0].x)*tmp1-(B[N-2].x-B[N-1].x)*tmp2;
				A[N-1][1]=(B[N-1].y-B[0].y)*tmp1-(B[N-2].y-B[N-1].y)*tmp2;
				D[N-1][0]=0.5*((R2[N-1]-R2[0]+tmp2*tmp2)*tmp1-(R2[N-2]-R2[N-1]-tmp1*tmp1)*tmp2);
				// MATH:
				if(!invMatrixA())
					return FALSE;
				mulMatrix(invA,D,X,cols,rows,1); // X = Ainv* D
				out.x=X[0][0];
				out.y=X[1][0];
				// JACOBIENS:
				l1=out.dist(B[0]);
				J=mJ[0].elems;
				a1=	invA[0][N-1]*(L[N-2]-L[N-1])-
					invA[0][0]*(L[N-1]-L[1])+
					invA[0][1]*(L[1]-L[2]);
				a2=	invA[1][N-1]*(L[N-2]-L[N-1])-
					invA[1][0]*(L[N-1]-L[1])+
					invA[1][1]*(L[1]-L[2]);
				lx=out.x-B[0].x;
				ly=out.y-B[0].y;
				J[0][0]=lx*a1;	J[0][1]=ly*a1;	J[0][2]=-l1*a1;
				J[1][0]=lx*a2;	J[1][1]=ly*a2;	J[1][2]=-l1*a2;
				mulMatrixTrans(J,mP[0],Px,2,3);
				M[0]=SQUARE_ELEMS(J)/(2*li*li);
				J=mJ[1].elems;
				a1=	invA[0][0]*(L[N-1]-L[0])-
					invA[0][1]*(L[0]-L[2])+
					invA[0][2]*(L[2]-L[3]);
				a2=	invA[1][0]*(L[N-1]-L[0])-
					invA[1][1]*(L[0]-L[2])+
					invA[1][2]*(L[2]-L[3]);
				li=l1-(L[0]-L[1]);
				lx=out.x-B[1].x;
				ly=out.y-B[1].y;
				J[0][0]=lx*a1; 
				J[0][1]=ly*a1; 
				J[0][2]=-li*a1;
				J[1][0]=lx*a2; 
				J[1][1]=ly*a2; 
				J[1][2]=-li*a2;
				mulMatrixTrans(J,mP[1],Ptmp,2,3);
				addMatrix(Ptmp,Px,Px,2,2);
				M[1]=SQUARE_ELEMS(J)/(2*li*li);
				for(i=2;i<N-2;i++)
				{
					J=mJ[i].elems;
					a1=	invA[0][i-1]*(L[i-2]-L[i-1])-
						invA[0][i]*(L[i-1]-L[i+1])+
						invA[0][i+1]*(L[i+1]-L[i+2]);
					a2=	invA[1][i-1]*(L[i-2]-L[i-1])-
						invA[1][i]*(L[i-1]-L[i+1])+
						invA[1][i+1]*(L[i+1]-L[i+2]);
					li=l1-(L[0]-L[i]);
					lx=out.x-B[i].x;
					ly=out.y-B[i].y;
					J[0][0]=lx*a1;	J[0][1]=ly*a1;	J[0][2]=-li*a1;
					J[1][0]=lx*a2;	J[1][1]=ly*a2;	J[1][2]=-li*a2;
					mulMatrixTrans(J,mP[i],Ptmp,2,3);
					addMatrix(Ptmp,Px,Px,2,2);
					M[i]=SQUARE_ELEMS(J)/(2*li*li);
				}
				J=mJ[N-2].elems;
				a1=	invA[0][N-3]*(L[N-4]-L[N-3])-
					invA[0][N-2]*(L[N-3]-L[N-1])+
					invA[0][N-1]*(L[N-1]-L[0]);
				a2=	invA[1][N-3]*(L[N-4]-L[N-3])-
					invA[1][N-2]*(L[N-3]-L[N-1])+
					invA[1][N-1]*(L[N-1]-L[0]);
				li=l1-(L[0]-L[N-2]);
				lx=out.x-B[N-2].x;
				ly=out.y-B[N-2].y;
				J[0][0]=lx*a1; 
				J[0][1]=ly*a1; 
				J[0][2]=-li*a1;
				J[1][0]=lx*a2; 
				J[1][1]=ly*a2; 
				J[1][2]=-li*a2;
				mulMatrixTrans(J,mP[N-2],Ptmp,2,3);
				addMatrix(Ptmp,Px,Px,2,2);
				M[N-2]=SQUARE_ELEMS(J)/(2*li*li);
				J=mJ[N-1].elems;
				a1=	invA[0][N-2]*(L[N-3]-L[N-2])-
					invA[0][N-1]*(L[N-2]-L[0])+
					invA[0][0]*(L[0]-L[1]);
				a2=	invA[1][N-2]*(L[N-3]-L[N-2])-
					invA[1][N-1]*(L[N-2]-L[0])+
					invA[1][0]*(L[0]-L[1]);
				li=l1-(L[0]-L[N-1]);
				lx=out.x-B[N-1].x;
				ly=out.y-B[N-1].y;
				J[0][0]=lx*a1;	J[0][1]=ly*a1;	J[0][2]=-li*a1;
				J[1][0]=lx*a2;	J[1][1]=ly*a2;	J[1][2]=-li*a2;
				mulMatrixTrans(J,mP[N-1],Ptmp,2,3);
				addMatrix(Ptmp,Px,Px,2,2);
				M[N-1]=SQUARE_ELEMS(J)/(2*li*li);
			}
			break;
			case MODEL_S_TDOA2:
			{
				// FILL:
				for(i=0;i<rows-1;i++)
				{
					D[i][0]=0.5*(R2[i]-L[i]*L[i]-R2[i+1]+L[i+1]*L[i+1]);
					A[i][2]=L[i]-L[i+1];
				}
				D[rows-1][0]=0.5*(R2[rows-1]-L[rows-1]*L[rows-1]-R2[0]+L[0]*L[0]);
				A[rows-1][2]=L[rows-1]-L[0];
				// MATH:
				if(!invMatrixA())
					return FALSE;
				mulMatrix(invA,D,X,cols,rows,1); // X = Ainv* D
				out.x=X[0][0];
				out.y=X[1][0];
				l1=X[2][0];
				// JACOBIENS:
				J=mJ[0].elems;
				a1=invA[0][N-1]-invA[0][0];
				a2=invA[1][N-1]-invA[1][0];
				lx=out.x-B[0].x;
				ly=out.y-B[0].y;
				li=L[0];
				J[0][0]=lx*a1;	J[0][1]=ly*a1;	J[0][2]=li*a1;
				J[1][0]=lx*a2;	J[1][1]=ly*a2;	J[1][2]=li*a2;
				mulMatrixTrans(J,mP[0],Px,2,3);
				M[0]=SQUARE_ELEMS(J)/(2*li*li);
				for(i=1;i<N;i++)
				{
					J=mJ[i].elems;
					a1=invA[0][i-1]-invA[0][i];
					a2=invA[1][i-1]-invA[1][i];
					lx=out.x-B[i].x;
					ly=out.y-B[i].y;
					li=L[i];
					J[0][0]=lx*a1;	J[0][1]=ly*a1;	J[0][2]=li*a1;
					J[1][0]=lx*a2;	J[1][1]=ly*a2;	J[1][2]=li*a2;
					mulMatrixTrans(J,mP[i],Ptmp,2,3);
					addMatrix(Ptmp,Px,Px,2,2);
					M[i]=SQUARE_ELEMS(J)/(2*li*li);
				}
			}
			break;
			default:
				assert(FALSE);
		}
		var.x=Px[0][0];
		var.y=Px[1][1];
		covar=Px[0][1];
	}
	else // Solution by Optimization
	{
		double	estRprev[2],dist=0,estR[2],tmpR[2],step=0,opti,
				Jg[2][2],tmpJ[2][3],invJg[2][2],Jxx,Jyy,Jxy,lidi,lxdi,lydi;
		BOOL buildRoute,JOK=FALSE,enhanced=FALSE;
		IterInfo *iterInfo;
		
		processing=TRUE;
		if(buildRoute=(routing&&bRoute.checked))
			tmpRoute.destroy();
		
		estRprev[0]=estR[0]=p[POS_START].x;
		estRprev[1]=estR[1]=p[POS_START].y;
		switch(model)
		{
			case MODEL_ITER_A2:
				enhanced=TRUE;
			case MODEL_ITER_A: // ENHANCED
			{
				double	invP[2][2],estPi[2][2],invPi[2][2],
						estP[2][2],estRi[2],Ox,Oy,**P,rO,
						li,deti,detr,Po[2][2],Jo[2][2],Psum[2][2],*L=Lr;

				ClearMemory(estP,sizeof(estP));
				for(j=0;j<maxIterCount;j++)
				{
					if(optim)
					{
						ClearMemory(tmpR,sizeof(tmpR));
						ClearMemory(invP,sizeof(invP));
						for(i=0,e=0,dex=0,dey=0;i<N;i++)
						{
							li=L[i];
							P=mP[i].elems;
							// Oi = (Rk-1 - Ri) / ||Rk-1 - Ri||
							Ox=estRprev[0]-B[i].x;
							Oy=estRprev[1]-B[i].y;
							rO=INV(getLength(Ox,Oy));
							Ox*=rO;
							Oy*=rO;
							// estRi = B + li * Oi
							estRi[0]=B[i].x+li*Ox;
							estRi[1]=B[i].y+li*Oy;
							if(enhanced)
							{
								// P2 = vi*Psum*vi
								Jo[0][0]=				(1-Ox*Ox)*rO;
								Jo[0][1]=Jo[1][0]=		-(Ox*Oy)*rO;
								Jo[1][1]=				(1-Oy*Oy)*rO;
								Psum[0][0]=				estP[0][0]+P[0][0];
								Psum[0][1]=Psum[1][0]=	estP[0][1]+P[0][1];
								Psum[1][1]=				estP[1][1]+P[1][1];
								mulMatrixTrans(Jo,Psum,Po);
								// P
								estPi[0][0]=			P[0][0]+P[2][2]*Ox*Ox+(li*li+P[2][2])*Po[0][0];
								estPi[0][1]=estPi[1][0]=P[0][1]+P[2][2]*Ox*Oy+(li*li+P[2][2])*Po[0][1];
								estPi[1][1]=			P[1][1]+P[2][2]*Oy*Oy+(li*li+P[2][2])*Po[1][1];
							}
							else
							{
								// P = bPi + VarLi * Oi * Oi'
								estPi[0][0]=			P[0][0]+P[2][2]*Ox*Ox;
								estPi[0][1]=estPi[1][0]=P[0][1]+P[2][2]*Ox*Oy;
								estPi[1][1]=			P[1][1]+P[2][2]*Oy*Oy;
							}
							// det(P)
							deti=estPi[0][0]*estPi[1][1]-estPi[0][1]*estPi[1][0];
							if(!deti)
								return FALSE;
							detr=INV(deti);
							// inv(P)
							invPi[0][0]=			 estPi[1][1]*detr;
							invPi[0][1]=invPi[1][0]=-estPi[0][1]*detr;
							invPi[1][1]=			 estPi[0][0]*detr;
							// estP^-1 = Sum (P^-1)
							invP[0][0]+=			invPi[0][0];
							invP[0][1]=invP[1][0]+=	invPi[0][1];
							invP[1][1]+=			invPi[1][1];
							// tmpR = Sum (P^-1 * estRi)
							tmpR[0]+=invPi[0][0]*estRi[0]+invPi[0][1]*estRi[1];
							tmpR[1]+=invPi[1][0]*estRi[0]+invPi[1][1]*estRi[1];
							// pdf
							lx=estRprev[0]-estRi[0];
							ly=estRprev[1]-estRi[1];
							e+=lx*lx*invPi[0][0]+2*lx*ly*invPi[0][1]+ly*ly*invPi[1][1];
							dex+=2*(lx*invPi[0][0]+ly*invPi[0][1]);
							dey+=2*(ly*invPi[1][1]+lx*invPi[0][1]);
						}
						invMatrix(invP,estP);
						// estR = estP * tmpR
						estR[0]=estP[0][0]*tmpR[0]+estP[0][1]*tmpR[1];
						estR[1]=estP[1][0]*tmpR[0]+estP[1][1]*tmpR[1];
					}
					else
					{
						ClearMemory(estR,sizeof(estR));
						for(i=0;i<N;i++)
						{
							// Oi = (Rk-1 - Ri) / ||Rk-1 - Ri||
							Ox=estRprev[0]-B[i].x;
							Oy=estRprev[1]-B[i].y;
							rO=INV(getLength(Ox,Oy));
							Ox*=rO;
							Oy*=rO;
							estR[0]+=B[i].x+L[i]*Ox;
							estR[1]+=B[i].y+L[i]*Oy;
						}
						estR[0]*=kBeacons;
						estR[1]*=kBeacons;
					}
					step=getLength(estR[0]-estRprev[0],estR[1]-estRprev[1]);
					if(buildRoute)
					{
						iterInfo=tmpRoute.add();
						iterInfo->count=(j+1);
						iterInfo->pos.x=estR[0];
						iterInfo->pos.y=estR[1];
						iterInfo->step=step;
						iterInfo->route=(dist+=step);
						iterInfo->error=getLength(x-estR[0],y-estR[1]);
						iterInfo->var=estP[0][0]+estP[1][1];
						iterInfo->func=e;
						iterInfo->grad.x=dex;
						iterInfo->grad.y=dey;
					}
					if(step<minIterStep)
						break;
					estRprev[0]=estR[0];
					estRprev[1]=estR[1];
				}
			}
			break;
			case MODEL_ITER_G:
			{
				for(j=0;j<maxIterCount;j++)
				{
					for(i=0,dex=0,dey=0,Jxx=0,Jyy=0,Jxy=0,e=0;i<N;i++)
					{
						li=L[i];
						opti=O[i];
						lx=estR[0]-B[i].x;
						ly=estR[1]-B[i].y;
						di=getLength(lx,ly);
						e+=pow2(di-li)*opti;
						double coef=INV(di);
						lidi=coef*li;
						lxdi=coef*lx;
						lydi=coef*ly;
						dex+=opti*(1.0-lidi)*lx;
						dey+=opti*(1.0-lidi)*ly;
						Jxx+=opti*(1.0-lidi*(1.0-pow2(lxdi)));
						Jyy+=opti*(1.0-lidi*(1.0-pow2(lydi)));
						Jxy+=opti*lidi*lxdi*lydi;
					}
					Jg[0][0]=Jxx;
					Jg[1][1]=Jyy;
					Jg[0][1]=Jg[1][0]=Jxy;
					invMatrix(Jg,invJg);
					JOK=TRUE;
					lx=-(invJg[0][0]*dex+invJg[0][1]*dey);
					ly=-(invJg[0][1]*dex+invJg[1][1]*dey);
					estR[0]+=lx;
					estR[1]+=ly;
					step=getLength(lx,ly);
					if(buildRoute)
					{
						iterInfo=tmpRoute.add();
						iterInfo->count=(j+1);
						iterInfo->pos.x=estR[0];
						iterInfo->pos.y=estR[1];
						iterInfo->step=step;
						iterInfo->route=(dist+=step);
						iterInfo->error=getLength(x-estR[0],y-estR[1]);
						iterInfo->func=e;
						iterInfo->grad.x=dex;
						iterInfo->grad.y=dey;
					}
					if(step<minIterStep)
						break;
				}
			}
			break;
			case MODEL_ITER_Q:
			{
				double quad[5][3],minE,cx,cy;
				int minQ,q;
				
				step=areaSide;
				
				quad[0][0]=estR[0];
				quad[0][1]=estR[1];
				quad[0][2]=0;
				for(i=0;i<N;i++)
					ADD_QUAD_ENERGY(0,i);
				minE=quad[0][2];
				minQ=0;
				for(j=0;j<maxIterCount;j++)
				{
					step*=(minQ==0)?0.5:1.1;
				
					// buildQuad
					cx=quad[minQ][0];
					cy=quad[minQ][1];
					// center
					quad[0][0]=cx;
					quad[0][1]=cy;
					// left top
					quad[1][0]=cx-0.5*step;
					quad[1][1]=cy-0.5*step;
					// right top
					quad[2][0]=cx+0.5*step;
					quad[2][1]=cy-0.5*step;
					// left bottom
					quad[3][0]=cx-0.5*step;
					quad[3][1]=cy+0.5*step;
					// right bottom
					quad[4][0]=cx+0.5*step;
					quad[4][1]=cy+0.5*step;
					switch(minQ)
					{
					case 1:
						quad[3][2]=quad[0][2];
						quad[1][2]=quad[2][2]=quad[4][2]=0;
						for(i=0;i<N;i++)
						{
							ADD_QUAD_ENERGY(1,i);
							ADD_QUAD_ENERGY(2,i);
							ADD_QUAD_ENERGY(4,i);
						}
						break;
					case 2:
						quad[4][2]=quad[0][2];
						quad[1][2]=quad[2][2]=quad[3][2]=0;
						for(i=0;i<N;i++)
						{
							ADD_QUAD_ENERGY(1,i);
							ADD_QUAD_ENERGY(2,i);
							ADD_QUAD_ENERGY(3,i);
						}
						break;
					case 3:
						quad[1][2]=quad[0][2];
						quad[2][2]=quad[3][2]=quad[4][2]=0;
						for(i=0;i<N;i++)
						{
							ADD_QUAD_ENERGY(2,i);
							ADD_QUAD_ENERGY(3,i);
							ADD_QUAD_ENERGY(4,i);
						}
						break;
					case 4:
						quad[2][2]=quad[0][2];
						quad[1][2]=quad[3][2]=quad[4][2]=0;
						for(i=0;i<N;i++)
						{
							ADD_QUAD_ENERGY(1,i);
							ADD_QUAD_ENERGY(3,i);
							ADD_QUAD_ENERGY(4,i);
						}
						break;
					default:
						quad[1][2]=quad[2][2]=quad[3][2]=quad[4][2]=0;
						for(i=0;i<N;i++)
						{
							ADD_QUAD_ENERGY(1,i);
							ADD_QUAD_ENERGY(2,i);
							ADD_QUAD_ENERGY(3,i);
							ADD_QUAD_ENERGY(4,i);
						}
						break;
					}
					minQ=0;
					quad[0][2]=minE;
					for(q=1;q<5;q++)
						if(quad[q][2]<quad[minQ][2])
							minQ=q;
					minE=quad[minQ][2];
					estR[0]=quad[minQ][0];
					estR[1]=quad[minQ][1];
					if(buildRoute)
					{
						iterInfo=tmpRoute.add();
						iterInfo->count=(j+1);
						iterInfo->pos.x=estR[0];
						iterInfo->pos.y=estR[1];
						iterInfo->step=step;
						iterInfo->route=(dist+=getLength(estRprev[0]-estR[0],estRprev[1]-estR[1]));
						estRprev[0]=estR[0];
						estRprev[1]=estR[1];
						iterInfo->error=getLength(x-estR[0],y-estR[1]);
						iterInfo->func=minE;
						for(i=0,dex=0,dey=0;i<N;i++)
						{
							double d=B[i].dist(estR[0],estR[1]);
							if(d!=L[i])
							{
								tmp=(1.0-L[i]/d)*O[i];
								dex+=tmp*(x-B[i].x);
								dey+=tmp*(y-B[i].y);
							}
						}
						iterInfo->grad.x=2*dex;
						iterInfo->grad.y=2*dey;
					}
					if(step<minIterStep)
						break;
				}
			}
			break;
			default:
				assert(FALSE);
		}
		out.x=estR[0];
		out.y=estR[1];
		route.destroy();
		if(buildRoute)
			tmpRoute.transfer(route);
		if(!JOK)
		{
			for(i=0,dex=0,dey=0,Jxx=0,Jyy=0,Jxy=0,e=0;i<N;i++)
			{
				li=L[i];
				lx=estR[0]-B[i].x;
				ly=estR[1]-B[i].y;
				di=getLength(lx,ly);
				opti=O[i];
				e+=pow2(di-li)*opti;
				double coef=INV(di);
				lidi=coef*li;
				lxdi=coef*lx;
				lydi=coef*ly;
				dex+=opti*(1.0-lidi)*lx;
				dey+=opti*(1.0-lidi)*ly;
				Jxx+=opti*(1.0-lidi*(1.0-pow2(lxdi)));
				Jyy+=opti*(1.0-lidi*(1.0-pow2(lydi)));
				Jxy+=opti*lidi*lxdi*lydi;
			}
			Jg[0][0]=Jxx;
			Jg[1][1]=Jyy;
			Jg[0][1]=Jg[1][0]=Jxy;
			invMatrix(Jg,invJg);
		}
		mPx.clear();
		for(i=0;i<N;i++)
		{
			li=L[i];
			opti=O[i];
			J=mJ[i].elems;
			lx=estR[0]-B[i].x;
			ly=estR[1]-B[i].y;
			di=getLength(lx,ly);
			double coef=INV(di);
			lidi=coef*li;
			lxdi=coef*lx;
			lydi=coef*ly;
			tmpJ[0][0]=				 opti*(1.0-lidi*(1.0-pow2(lxdi)));
			tmpJ[1][1]=				 opti*(1.0-lidi*(1.0-pow2(lydi)));
			tmpJ[0][1]=tmpJ[1][0]=	 opti*lidi*lxdi*lydi; 
			tmpJ[0][2]=				-opti*lxdi;
			tmpJ[1][2]=				-opti*lydi;
			J[0][0]=invJg[0][0]*tmpJ[0][0]+invJg[0][1]*tmpJ[1][0];
			J[0][1]=invJg[0][0]*tmpJ[0][1]+invJg[0][1]*tmpJ[1][1];
			J[0][2]=invJg[0][0]*tmpJ[0][2]+invJg[0][1]*tmpJ[1][2];
			J[1][0]=invJg[1][0]*tmpJ[0][0]+invJg[1][1]*tmpJ[1][0];
			J[1][1]=invJg[1][0]*tmpJ[0][1]+invJg[1][1]*tmpJ[1][1];
			J[1][2]=invJg[1][0]*tmpJ[0][2]+invJg[1][1]*tmpJ[1][2];
			mulMatrixTrans(J,mP[i],Ptmp,2,3);
			addMatrix(Ptmp,Px,Px,2,2);
			M[i]=SQUARE_ELEMS(J)/(2*li*li);
		}
		iterOut.count=(j+1);
		iterOut.pos.set(estR[0],estR[1]);
		p[POS_END].copy(iterOut.pos);
		iterOut.step=step;
		iterOut.error=getLength(x-estR[0],y-estR[1]);
		iterOut.var=mPx.trace();
		iterOut.route=dist;
		iterOut.func=e;
		iterOut.grad.set(dex,dey);

		var.x=Px[0][0];
		var.y=Px[1][1];
		covar=Px[0][1];
		if(buildRoute)
		{
			if(route.length&&graph==GRAPH_ROUTE)
			{
				IterObject iterObj;
				setGraph(GRAPH_ROUTE,route.length);
				for(iterObj=route.first;iterObj;iterObj=iterObj->next)
				{
					ADDVAL(CHNL_ITER_ROUTE,		iterObj->value.route);
					ADDVAL(CHNL_ITER_COUNT,		iterObj->value.count);
					ADDVAL(CHNL_ITER_STEP,		iterObj->value.step);
					ADDVAL(CHNL_ITER_ERROR,		iterObj->value.error);
					ADDVAL(CHNL_ITER_VAR,		iterObj->value.var);
					ADDVAL(CHNL_ITER_FUNC,		iterObj->value.func);
					ADDVAL(CHNL_ITER_GRAD,		getLength(iterObj->value.grad.x,iterObj->value.grad.y));
					ADDVAL(CHNL_ITER_GRAD_X,	iterObj->value.grad.x);
					ADDVAL(CHNL_ITER_GRAD_Y,	iterObj->value.grad.y);
				}
				fg.graph.update();
			}
		}
		stopProcessing();
	}
	return TRUE;
}
BOOL Trilat::getLevel(double x, double y, double &level)
{
	int i;
	Point out,var;
	FullStat stat;
	double covar=0,x0,y0;
	Point *p0=NULL;
	IterInfo iterOut;

	switch(mapArg)
	{
	case MAP_ARG_BEACON:
		B[active].set(x,y);
		x=R.x;
		y=R.y;
		prefetch();
		break;
	case MAP_ARG_START:
		p0=p+POS_START;
		x0=p0->x;
		y0=p0->y;
		p0->set(x,y);
		x=R.x;
		y=R.y;
		break;
	}
	switch(mapType)
	{
		case MAP_TYPE_EST:
			if(!getStatEst(x,y,out,var,covar,iterOut,FALSE))
				return FALSE;
			break;
		case MAP_TYPE_NUM:
			if(!getStatNum(x,y,stat,FALSE))
				return FALSE;
			break;
		case MAP_TYPE_DIFF:
			if(!getStatNum(x,y,stat,FALSE)||!getStatEst(x,y,out,var,covar,iterOut,FALSE))
				return FALSE;
			break;
	}
	switch(map)
	{
		case MAP_CALC_ERROR:
			level=out.dist(x,y);
			break;
		case MAP_TRANS_FACTOR:
			level=Lr[active]*sqrt(2*M[active]);
			break;
		case MAP_TRANS_FULL:
			level=0;
			for(i=0;i<N;i++)
				level+=Lr[i]*sqrt(2*M[i]);
			break;
		case MAP_EST_VAR:
			level=var.x+var.y;
			break;
		case MAP_EST_VAR_X:
			level=var.x;
			break;
		case MAP_EST_VAR_Y:
			level=var.y;
			break;
		case MAP_EST_CORREL:
			level=getCorrel(covar,sqrt(var.x*var.y));
			break;
		case MAP_PAR_VAR:
			level=getParabVar(x,y);
			break;
		case MAP_PAR_ERR:
			level=var.x+var.y-getParabVar(x,y);
			break;
		case MAP_MASS:
			level=M[active];
			break;
		case MAP_FULLMASS:
			level=M[0];
			for(i=1;i<N;i++)
				level+=M[i];
			break;
		case MAP_GRAD_MASS:
			level=gradM[active].dist();
			break;
		case MAP_ITERS:
			level=iterOut.count;
			break;
		case MAP_ITER_MISS:
			level=iterErrRatio*100;
			break;
		case MAP_DIFF_VAR:
			level=stat.stdev.square()-(var.x+var.y);
			break;
		case MAP_DIFF_CORREL:
			level=stat.correl-getCorrel(covar,sqrt(var.x*var.y));
			break;
		case MAP_NUM_VAR:
			level=stat.stdev.square();
			break;
		case MAP_NUM_VAR_X:
			level=stat.stdev.x*stat.stdev.x;
			break;
		case MAP_NUM_VAR_Y:
			level=stat.stdev.y*stat.stdev.y;
			break;
		case MAP_NUM_CORREL:
			level=stat.correl;
			break;
		case MAP_MEAN:
			level=stat.mean.dist();
			break;
		case MAP_MEAN_X:
			level=stat.mean.x;
			break;
		case MAP_MEAN_Y:
			level=stat.mean.y;
			break;
		case MAP_EXCESS:
			level=stat.excess.dist();
			break;
		case MAP_EXCESS_X:
			level=stat.excess.x;
			break;
		case MAP_EXCESS_Y:
			level=stat.excess.y;
			break;
		case MAP_SKEW:
			level=stat.skew.dist();
			break;
		case MAP_SKEW_X:
			level=stat.skew.x;
			break;
		case MAP_SKEW_Y:
			level=stat.skew.y;
			break;
		case MAP_ENERGY:
			level=0;
			if(!R.equals(x,y))
				for(i=0;i<N;i++)
					level+=pow2(B[i].dist(R)-B[i].dist(x,y))*O[i];
			break;
		case MAP_GRAD_ENERGY:
		{
			if(R.equals(x,y))
			{
				level=0;
			}
			else
			{
				double d,lx=0,ly=0,dx,dy;
				Point *b;
				for(i=0;i<N;i++)
				{
					b=B+i;
					dx=x-b->x;
					dy=y-b->y;
					if(dx||dy)
					{
						d=(1-b->dist(R)/getLength(dx,dy))*O[i];
						lx+=d*dx;
						ly+=d*dy;
					}
				}
				level=2*getLength(lx,ly);
			}
			break;
		}
		case MAP_MLK:
		{
			double d,lx,ly,li,dx,dy,estPi[2][2],**P,deti;
			for(i=0,level=0;i<N;i++)
			{
				P=mP[i].elems;
				dx=p[POS_ITER].x-B[i].x;
				dy=p[POS_ITER].y-B[i].y;
				d=getLength(dx,dy);
				if(!d)
					return FALSE;
				dx/=d;
				dy/=d;
				li=B[i].dist(R);
				lx=B[i].x+li*dx;
				ly=B[i].y+li*dy;
				estPi[0][0]=			P[0][0]+P[2][2]*dx*dx;
				estPi[0][1]=estPi[1][0]=P[0][1]+P[2][2]*dx*dy;
				estPi[1][1]=			P[1][1]+P[2][2]*dy*dy;
				deti=estPi[0][0]*estPi[1][1]-estPi[0][1]*estPi[1][0];
				if(deti<=0)
					return FALSE;
				lx=x-lx;
				ly=y-ly;
				level+=(lx*lx*estPi[1][1]-2*lx*ly*estPi[0][1]+ly*ly*estPi[0][0])/deti;
			}
			break;
		}
		default:
			return FALSE;
	}
	if(p0)
		p0->set(x0,y0);
	return TRUE;
}
DWORD Trilat::run(__int64 action)
{
	switch(action)
	{
		case ACTION_GRAPH:
		{
			Lock lock(&run_sect);
			int index=fg.graph.markIndex;
			if(index>=0)
			{
				if(graph==GRAPH_ROUTE)
				{
					ListObject<IterInfo> *obj=route.objectAt(index);
					if(obj)
					{
						iterIndex=index;
						p[POS_ITER]=mark=obj->value.pos;
						if(mapArg==MAP_ARG_POINT&&map==MAP_MLK)
						{
							action=ACTION_MAP;
							break;
						}
					}
				}
				else
				{
					updateSectionMark();
				}
			}
			return ACTION_ERROR_NONE;
		}
	}
	DWORD error=ACTION_ERROR_NONE;
	double progstep;
	int active_beacon=0;
	BOOL revert_beacons=FALSE,recalc_on_success=TRUE;
	Point activePos;
	
	HANDLE current_thread=active_thread=GetCurrentThread();
	Lock lock(&run_sect);
	if(pending_action==action)
		pending_action=0;
	if(active_thread!=current_thread)
		return ACTION_ERROR_OLD;

#define CHECK_THREAD()\
	if(active_thread!=current_thread){\
		error = (active_thread ? ACTION_ERROR_INTERRUPT : ACTION_ERROR_STOPPED);\
		goto FINISH_THREAD;}
#define CHECK_INIT(condition)\
	if(!(condition)){\
		error=ACTION_ERROR_INIT;\
		goto FINISH_THREAD;}

	processing=TRUE;
	progress=0;
	actionTimer.restart();

	switch(action)
	{
		case ACTION_MAP:
		{
			int y,x,i,a,levCount=0,indmin=0,indmax=0;
			double level,_levmid=0,_levmin=0,_levmax=0;
			BYTE color,*ecolor,*tcolor,*comp;
			
			a=areaSide;
			level=0;
			progstep=INV(a);
			ecolor=INT_TO_BYTE(mapErrorColor),
			tcolor=INT_TO_BYTE(mapThresColor),
			comp=image.data;

			if(mapType==MAP_TYPE_DISTRIB)
			{
				recalc_on_success=FALSE;
				if(!mapChanged)
					recalc();
				CHECK_THREAD();
				vLevels.clear();
				_levmax=_levmin=_levmid=0;
				indmin=indmax=0;
				switch(map)
				{
					case MAP_DISTRIB_NUM:
					{
						CHECK_INIT(K);

						level=INV(K);
						for(i=0;i<K;i++)
						{
							CHECK_THREAD();
							x=round(R.x+distribs[i].x);
							if(x>=0&&x<a)
							{
								y=round(R.y+distribs[i].y);
								if(y>=0&&y<a)
									levels[a*y+x]+=level;
							}
							progress+=level;
						}
						for(i=0;i<areaSize;i++)
						{
							_levmid+=level=levels[i];
							if(_levmin>level)
							{
								indmin=i;
								_levmin=level;
							}
							else if(_levmax<level)
							{
								indmax=i;
								_levmax=level;
							}
						}	
					}
					break;
					case MAP_DISTRIB_EST:
					{
						double deti,rdet,dx,dy,mx,my,vx,vy,vxy,coef;
						
						mx=estR.x;
						my=estR.y;
						vx=estVar.x;
						vy=estVar.y;
						vxy=estCovar;
						deti=vx*vy-vxy*vxy;
						if(!deti)
							break;
						rdet=INV(deti);
						coef=INV(2*PI*sqrt(deti));
						for(y=0,i=0;y<a;y++,progress+=progstep)
						{
							for(x=0;x<a;x++,i++)
							{
								CHECK_THREAD();
								dx=x-mx;
								dy=y-my;
								_levmid+=level=levels[i]=coef*exp(-0.5*(dx*dx*vy-2*dx*dy*vxy+dy*dy*vx)*rdet);
								if(_levmin>level)
								{
									indmin=i;
									_levmin=level;
								}
								else if(_levmax<level)
								{
									indmax=i;
									_levmax=level;
								}
							}
						}
					}
					break;
					case MAP_DISTRIB_ERR:
					{
						// compute num:
						CHECK_INIT(K);
						level=INV(K);
						double maxLev=0,tmpLev;
						for(i=0;i<K;i++,progress+=0.5*level)
						{
							CHECK_THREAD();
							x=round(R.x+distribs[i].x);
							if(x>=0&&x<a)
							{
								y=round(R.y+distribs[i].y);
								if(y>=0&&y<a)
								{
									tmpLev=(levels[a*y+x]+=level);
									if(maxLev<tmpLev)
										maxLev=tmpLev;
								}
							}
						}
						// compute est and errors:
						CHECK_THREAD();
						if(maxLev)
						{
							double deti,rdet,dx,dy,mx,my,vx,vy,vxy,coef,kErr=100/maxLev;
							mx=estR.x;
							my=estR.y;
							vx=estVar.x;
							vy=estVar.y;
							vxy=estCovar;
							deti=vx*vy-vxy*vxy;
							if(!deti)
								break;
							rdet=INV(deti);
							coef=INV(2*PI*sqrt(deti));
							for(y=0,i=0;y<a;y++,progress+=0.5*progstep)
							{
								for(x=0;x<a;x++,i++)
								{
									CHECK_THREAD();
									dx=x-mx;
									dy=y-my;
									_levmid+=level=(levels[i]=kErr*abs(levels[i]-coef*exp(-0.5*(dx*dx*vy-2*dx*dy*vxy+dy*dy*vx)*rdet)));
									if(_levmin>level)
									{
										indmin=i;
										_levmin=level;
									}
									else if(_levmax<level)
									{
										indmax=i;
										_levmax=level;
									}
								}
							}
						}
					}
					break;
				}
				_levmid=0.5*(_levmax+_levmin);
			}
			else
			{
				if(mapArg==MAP_ARG_BEACON)
				{
					revert_beacons=TRUE;
					active_beacon=active;
					activePos.copy(B[active]);
					//PRINT(_T("\nPUSH b: %2d, x:%6.2f, y:%6.2f\n"),active,activePos.x,activePos.y);
				}
				for(y=0,i=0;y<a;y++,progress+=progstep)
				{
					for(x=0;x<a;x++,i++)
					{
						CHECK_THREAD();
						
						if(!getLevel(x,y,level))
						{
							levels[i]=ERROR_LEVEL;
							//PRINT(_T("Error level at (%d, %d)\n"),x,y)
							continue;
						}
						levels[i]=level;	
						if(levCount)
						{
							_levmid+=level;
							if(_levmin>level)
							{
								indmin=i;
								_levmin=level;
							}
							else if(_levmax<level)
							{
								indmax=i;
								_levmax=level;
							}
						}
						else
						{
							_levmid=_levmin=_levmax=level;
							indmin=indmax=i;
						}
						levCount++;
					}
				}
				_levmid/=levCount;
			}
			if(bMapLims.checked&&(mapmax>mapmin))
			{
				double r=255*INV(mapmax-mapmin);
				for(i=0;i<areaSize;i++,comp+=3)
				{
					level=levels[i];
					if(level==ERROR_LEVEL)
					{
						*comp=ecolor[RED];
						*(comp+1)=ecolor[GREEN];
						*(comp+2)=ecolor[BLUE];
					}
					else if(level>thresInf&&level<thresSup)
					{
						*comp=tcolor[RED];
						*(comp+1)=tcolor[GREEN];
						*(comp+2)=tcolor[BLUE];
					}
					else
					{
						if(level<=mapmin)
							color=0;
						else if(level>=mapmax)
							color=255;
						else
							color=(BYTE)round((level-mapmin)*r);
						*comp=*(comp+1)=*(comp+2)=color;
					}
				}
			}
			else
			{
				if(_levmax==_levmin)
				{
					image.clear();
				}
				else
				{
					double r1,r2;
					r1=127.5*INV(_levmid-_levmin);
					r2=127.5*INV(_levmax-_levmid);
					for(i=0;i<areaSize;i++,comp+=3)
					{
						level=levels[i];
						if(level==ERROR_LEVEL)
						{
							*comp=ecolor[RED];
							*(comp+1)=ecolor[GREEN];
							*(comp+2)=ecolor[BLUE];
						}
						else if(level>thresInf&&level<thresSup)
						{
							*comp=tcolor[RED];
							*(comp+1)=tcolor[GREEN];
							*(comp+2)=tcolor[BLUE];
						}
						else
						{
							if(level<_levmid)
								color=(BYTE)round((level-_levmin)*r1);
							else
								color=(BYTE)round(127.5+(level-_levmid)*r2);
							*comp=*(comp+1)=*(comp+2)=color;
						}
					}
					mapmin=_levmin;
					mapmax=_levmax;
					showMapLims();
				}
			}
			levmin=_levmin;
			levmax=_levmax;
			levmid=_levmid;
			p[POS_LEVMIN].x=indmin%a;
			p[POS_LEVMIN].y=indmin/a;
			p[POS_LEVMAX].x=indmax%a;
			p[POS_LEVMAX].y=indmax/a;
			
			CHECK_THREAD();
			if(revert_beacons&&active_beacon==active)
			{
				revert_beacons=FALSE;
				B[active].copy(activePos);
				prefetch();
			}
			updateSection();
			mapChanged=FALSE;
			CHECK_THREAD();
		}
		break;
		case ACTION_FIT:
		{
			CHECK_INIT(trajectory.length);

			progstep=INV(trajectory.length);
			List<Point> tmp;
			PointObject trajObj;
			tmp.copy(trajectory);
			trajectory.destroy();
			setGraph(GRAPH_CUST,tmp.length);
			for(trajObj=tmp.first;trajObj;trajObj=trajObj->next)
			{
				CHECK_THREAD();

				R.copy(trajObj->value);
				showPos();
				progress+=progstep;
				recalc(TRUE);
			}
		}
		break;
		case ACTION_SCRIPT:
		{
			int i=0,scr=chScript.getSelectedIndex();
			BOOL toggle=FALSE;
	
			CHECK_INIT(scr>=0&&scr<SCRIPT_COUNT);
			
			clearCust();
			useCustArg=TRUE;
			progress=0;
			progstep=INV(points);
				
			switch(scr)
			{
				case SCRIPT_NUM_EST:
				{
					while(i<points)
					{
						CHECK_THREAD();
						progress+=progstep;
						custArg=pown(1.1,i)*100;
						setArray(N,(int)custArg);
						updateStats();
						recalc(TRUE);
						i++;
					};
					break;
				}
				case SCRIPT_SYM_BEACON:
				{
					double startN=N,tmpN;
					custArg=N;
					recalc(TRUE);
					i++;

					while(i<points)
					{
						CHECK_THREAD();
						progress+=progstep;
						tmpN=custArg;
						custArg=remap(i,0,points,startN,3.0);
						if((int)custArg==(int)tmpN)
						{
							addCust();
							continue;
						}
						setArray((int)custArg,K,keepErrors);
						locateBeacons();
						updateStats(FALSE);
						recalc(TRUE);i++;
					}
					break;
				}
				case SCRIPT_STAT_CHANGE_L:
					toggle=TRUE;
				case SCRIPT_STAT_CHANGE_XY:
				{
					double stdevXY0=stdevXY,stdevL0=stdevL;
					float k0,k1=1,k,k2;
					int sample,beacon;
					StatBeacon *sb;
					MATRIX P;
					Error *berror;

					if(!errors)
						break;

					while(i<points)
					{
						CHECK_THREAD();
						progress+=progstep;

						k0=k1;
						k1=remap(i+1,0,points,0.0f,1.0f);
						k=k1/k0;
						k2=k*k;
						
						custArg=toggle?(stdevL*=k):(stdevXY*=k);
						for(beacon=0;beacon<N;beacon++)
						{
							sb=stats+beacon;
							P=mP[beacon].elems;
							if(toggle)
							{
								sb->devl*=k;
								P[2][2]*=k2;
							}
							else
							{
								sb->devx*=k;
								sb->devy*=k;
								P[0][0]*=k2;
								P[1][1]*=k2;
								P[0][1]=P[1][0]*=k2;
							}
							// berror generation:
							if(errors)
							{
								for(sample=0;sample<K;sample++)
								{
									berror=errors[sample]+beacon;
									if(toggle)
									{
										berror->l*=k;
									}
									else
									{
										berror->x*=k;
										berror->y*=k;
									}
								}
							}
						}
						updateOpt();
						getCOG();

						/*
						for(beacon=0;beacon<N;beacon++)
						{
							if(errors)
							{
								for(sample=0;sample<K;sample++)
								{
									berror=errors[sample]+beacon;
									berror->x*=k;
									berror->y*=k;
								}
							}
						}
						*/

						recalc(TRUE);
						i++;
					}
					stdevXY=stdevXY0;
					stdevL=stdevL0;
					break;
				}
			}

			useCustArg=FALSE;
			fg.graph.update();
		}
		break;
		case ACTION_PLAN:
		{
			CHECK_INIT(model==MODEL_S_TOF);
			recalc_on_success=FALSE;
			int i,j,nextBeacon,count,planType = chPlan.getSelectedIndex();
			Point f,dir,oldDir,dir_t,dir_m,dir_c,dir_p,B0,pos0,pos1,pos,oldPos;
			double V0,Vmin,Vk,M0,Mmax,Mk=0,mod,d,l=0,sprod_stop=0,sprod_dir=0,step,dist,maxDist;
			BOOL buildRoute,ignoreDir,switchDir,overload=FALSE,success=FALSE,routing = TRUE,
				forceM=bForceM.checked,forceP=bForceP.checked,forceC=bForceC.checked;
			IterInfo iter;

			ClearStruct(iter);
			if(buildRoute=(routing&&bRoute.checked))
				route.destroy();
			do
			{
				// Find the next beacon
				maxDist=B[0].square(p[POS_TARGET]);
				j=0;
				for(i=1;i<N;i++)
				{
					dist=B[i].square(p[POS_TARGET]);
					if(maxDist<dist)
					{
						maxDist=dist;
						j=i;
					}
				}
				CHECK_THREAD();
				nextBeacon=j;
				setActiveBeacon(nextBeacon);
				pos0=p[POS_START]=R;
				B0=B[nextBeacon];
				PRINT(_T("[PLAN] Next beacon: %d\n"),j+1);
				if(!getParabCoef(TRUE))
				{
					error=TRUE;
					PRINT(_T("[PLAN] Cannot get parab coefs!\n"));
					break;
				}
				step=minIterStep;
				V0=getParabVar(pos0);
				Vmin=getParabVar(p[POS_ERRMIN]);
				M0=FGW;
				Mmax=getMassMax(nextBeacon);
				if(!Mmax)
				{
					error=TRUE;
					PRINT(_T("[PLAN] Cannot get maximum mass!\n"));
					break;
				}
				PRINT(_T("[PLAN] Start first pass: max mass %f\n"),Mmax);
				CHECK_THREAD();
				if(V0>Vmin&&M0<Mmax)
				{
					// FIRST RUN  *** *** *** *** ***
					oldPos=pos0;
					count=0;
					dir.set(0,0);
					ignoreDir=TRUE;
					switchDir=FALSE;
					for(;;)
					{
						//if(step<minIterStep)
						//	break;
						if(count>maxIterCount)
						{
							error=TRUE;
							PRINT(_T("[PLAN] Too many iterations: %d (max %d)\n"), count, maxIterCount);
							break;
						}
						CHECK_THREAD();
						count++;
						pos.x=oldPos.x+step*dir.x;
						pos.y=oldPos.y+step*dir.y;
						Vk=getParabVar(pos);
						// Ft - target force: PRIORITY *
						dir_t.x=p[POS_TARGET].x-pos.x;
						dir_t.y=p[POS_TARGET].y-pos.y;
						dist=d=dir_t.dist();
						dir_t.x/=d;
						dir_t.y/=d;
						f=dir_t;
						// Fp - precision force: PRIORITY **
						if(forceP)
						{
							dir_p.x=p[POS_ERRMIN].x-pos.x;
							dir_p.y=p[POS_ERRMIN].y-pos.y;
							d=dir_p.dist();
							dir_p.x/=d;
							dir_p.y/=d;
							mod=(Vk-Vmin)/(V0-Vmin);
							f.x+=mod*dir_p.x;
							f.y+=mod*dir_p.y;
						}
						// Fm - fgw force: PRIORITY ***
						if(forceM)
						{
							B[nextBeacon].copy(pos);
							Mk=getMassGrad(&dir_m,nextBeacon);
							B[nextBeacon].copy(B0);
							d=dir_m.dist();
							dir_m.x/=-d;
							dir_m.y/=-d;
							
							sprod_dir=dir_m.x*dir_t.x+dir_m.y*dir_t.y;
							if(!switchDir)
							{
								if(sprod_dir<criticSprodDir)
									dir_m=dir_t;
								else
									ignoreDir=switchDir=TRUE;
							}
							if(Mmax>Mk)
							{
								mod=(Mk/M0)*(Mmax-M0)/(Mmax-Mk);
								f.x+=mod*dir_m.x;
								f.y+=mod*dir_m.y;
							}
							else // area with supperior weith => infinite fgw force power
							{
								f=dir_m;
							}
						}
						mod=f.dist();
						double coef=INV(mod);
						dir.set(coef*f.x,coef*f.y);
						if(mod<minIterGrad)
						{
							PRINT(_T("[PLAN] First pass finished without success: force %f (min %f))\n"), mod, minIterGrad);
							break;
						}
						else if(ignoreDir)
						{
							ignoreDir=FALSE;
							sprod_stop=1;
						}
						else
						{
							sprod_stop=oldDir.x*dir.x+oldDir.y*dir.y;
							if(sprod_stop<criticSprodStop)
							{
								step*=sr;
								continue;
							}
						}
						if(bShowFirstPass.checked)
						{
							iter.route+=step;
							iter.step=step;
							iter.count=count;
							iter.error=dist;
							iter.var=Vk;
							iter.func=Mk;
							iter.grad=f;
							iter.pos=pos;
							iter.trig=sprod_dir;
							if(buildRoute)
								route.add(iter);
						}
						if(dist<minIterStep)
						{
							success=TRUE;
							PRINT(_T("[PLAN] First pass finished with success: dist %f (min %f))\n"), dist, minIterStep);
							break;
						}
						oldPos=pos;
						oldDir=dir;
					}
					if(error)
						break;
					// SECOND RUN  *** *** *** *** ***
					count=0;
					pos1=pos;
					oldPos=pos0;
					step=minIterStep;
					dir.set(0,0);
					ignoreDir=TRUE;
					switchDir=FALSE;
					for(;;)
					{
						CHECK_THREAD();
						count++;
						pos.x=oldPos.x+step*dir.x;
						pos.y=oldPos.y+step*dir.y;
						Vk=getParabVar(pos);
						// Ft - target force: PRIORITY *
						dir_t.x=pos1.x-pos.x;
						dir_t.y=pos1.y-pos.y;
						dist=d=dir_t.dist();
						dir_t.x/=d;
						dir_t.y/=d;
						f=dir_t;
						// Fc - obstacles force: PRIORITY ****
						if(forceC)
						{
							for(i=0;i<N;i++)
							{
								dir_c.x=pos.x-B[i].x;
								dir_c.y=pos.y-B[i].y;
								d=dir_c.dist();
								dir_c.x/=d;
								dir_c.y/=d;
								l=d-rc;
								if(l<la)
								{
									if(l<=0) // inside obstacle, impossible
									{
										if(ignoreDir)
										{
											PRINT(_T("[PLAN] Inside an obstacle when ignoring ditrection!\n"));
											error=TRUE;
										}
										else
											overload=TRUE;
										break;
									}
									overload=FALSE;
									mod=k_obst*(la-l)*(la-l)/l;
									f.x+=mod*dir_c.x;
									f.y+=mod*dir_c.y;
								}
							}
							if(error)
								break;
							if(overload)
							{
								step*=sr;
								continue;
							}
						}
						mod=f.dist();
						double coef=INV(mod);
						dir.set(coef*f.x,coef*f.y);
						if(mod<minIterGrad)
						{
							PRINT(_T("[PLAN] Second pass finished without success: force %f (min %f))\n"), mod, minIterGrad);
							break;
						}
						else if(ignoreDir)
						{
							ignoreDir=FALSE;
							sprod_stop=1;
						}
						else
						{
							sprod_stop=oldDir.x*dir.x+oldDir.y*dir.y;
							if(sprod_stop<criticSprodStop)
							{
								step*=sr;
								continue;
							}
						}
						if(!bShowFirstPass.checked)
						{
							iter.route+=step;
							iter.step=step;
							iter.count=count;
							iter.error=dist;
							iter.var=Vk;
							iter.grad=f;
							iter.pos=pos;
							iter.trig=sprod_stop;
							if(buildRoute)
								route.add(iter);
						}
						if(dist<minIterStep)
						{
							PRINT(_T("[PLAN] Second pass finished with success: dist %f (min %f))\n"), dist, minIterStep);
							break;
						}
						oldPos=pos;
						oldDir=dir;
					}
					if(error)
						break;
				}
				if(planType!=PLAN_TYPE_SIM)
				{
					R=pos;
					recalc(FALSE);
					swap(nextBeacon);
				}
			}
			while(planType==PLAN_TYPE_ALL&&!success);
			if(buildRoute)
			{
				IterObject iterObj;
				setGraph(GRAPH_ROUTE,route.length);
				for(iterObj=route.first;iterObj;iterObj=iterObj->next)
				{
					ADDVAL(CHNL_ITER_ROUTE,	iterObj->value.route);
					ADDVAL(CHNL_ITER_COUNT,	iterObj->value.count);
					ADDVAL(CHNL_ITER_STEP,	iterObj->value.step);
					ADDVAL(CHNL_ITER_FUNC,	iterObj->value.func);
					ADDVAL(CHNL_ITER_ERROR,	iterObj->value.error);
					ADDVAL(CHNL_ITER_VAR,	iterObj->value.var);
					ADDVAL(CHNL_ITER_TRIG,	iterObj->value.trig);
					ADDVAL(CHNL_ITER_GRAD,	iterObj->value.grad.dist());
					ADDVAL(CHNL_ITER_GRAD_X,iterObj->value.grad.x);
					ADDVAL(CHNL_ITER_GRAD_Y,iterObj->value.grad.y);
				}
				fg.graph.update();
				p[POS_END]=pos;
			}
			iterMotion=iter;
			recalc(FALSE);
		}
		break;
	}
FINISH_THREAD:
	if(revert_beacons&&active_beacon==active)
	{
		B[active].copy(activePos);
		prefetch();
	}
	if(!error)
	{
		if(recalc_on_success)
			recalc(FALSE);
		progress=1;
		actionTime=actionTimer.time();
	}
	if(active_thread==current_thread)
	{
		active_thread=NULL;
		wake();
	}
	stopProcessing();
	action_result=error;
	return error;
}
void Trilat::stop(BOOL wait, long line)
{
	_DEBUG_EXPR(__stopAtLine=line);
	line;
	statProcess=FALSE;
	active_thread=NULL;
	if(wait)
		run_sect.Wait();
}
void Trilat::startMap()
{
	if(mapType)
	{
		if(pending_action!=ACTION_MAP)
		{
			pending_action=ACTION_MAP;
			stop();
			start(ACTION_MAP);
		}
	}
	else
	{
		actionTimer.restart();
		recalc(bAllCust.checked);
		actionTime=actionTimer.time();
	}
}
void Trilat::reshape()
{
	PRINT(_T("Reshape -> l:%d, t:%d, w:%d, h:%d\n"),window.left,window.top,window.width,window.height);

	FlatApplication::reshape();
	adjustWindow();
}
BOOL Trilat::glCreate()
{
	FlatApplication::glCreate();
	lineGran=getLineWidthProps(&lineMin,&lineMax);
	glShadeModel(GL_SMOOTH);
	createLight(GL_LIGHT1,LightAmbient,LightDiffuse,LightPosition);

	PRINT(_T("\n\n--------------------- Graphics created.\n"));PRINTMEM();

	return TRUE;
}
void Trilat::allowParabEstim(BOOL newParabEstim)
{
	parabEstim=newParabEstim;
	bParabEstim.checked=newParabEstim;
	prefetch();
}
void Trilat::allowMotionEstim(BOOL newMotionEstim)
{
	motionEstim=newMotionEstim;
	bMotionEstim.checked=newMotionEstim;
	prefetch();
}
void Trilat::setMotionTarget(double x, double y)
{
	p[POS_TARGET].set(x,y);
	format.precision=2;
	tbTargetX.setDouble(x);
	tbTargetY.setDouble(y);
	format.precision=3;
}
void Trilat::setMotionParams(	double equalRegion,
								double criticRegion,
								double activeRegion,
								double speedReduct)
{
	if(equalRegion<=0)
		equalRegion=DEF_EQUAL_REGION;
	if(criticRegion<=0)
		criticRegion=DEF_CRITIC_REGION;
	if(speedReduct<=0||speedReduct>=1)
		speedReduct=DEF_SPEED_REDUCT;
	sr=speedReduct;
	lt=equalRegion;
	rc=criticRegion;
	if(activeRegion>equalRegion)
		la=activeRegion;
	else if(lt>1)
		la=lt*(1+2.0/(lt-1));
	else
		la=3*lt;
	k_obst=lt/pow2(la-lt);
	format.precision=2;
	tbMotionEqual.setDouble(lt);
	tbMotionCritic.setDouble(rc);
	tbMotionActive.setDouble(la);
	tbSpeedReduct.setDouble(sr);
	format.precision=3;
}
double Trilat::getMassGrad(Point *grad, int beacon)
{
	double omega,epsilon,Mk;
	int i,k,k_1;

	if(!grad)
		return 0;
	if(beacon<0)
		beacon=active;
	if(beacon>0)
	{
		k=beacon;
		k_1=beacon-1;
	}
	else
	{
		k=0;
		k_1=N-1;
	}

	// Build A0:
	mA0.clear();
	for(i=0;i<rows-1;i++)
	{
		A0[i][0]=B[i].x-B[i+1].x;
		A0[i][1]=B[i].y-B[i+1].y;
	}
	A0[rows-1][0]=B[rows-1].x-B[0].x;
	A0[rows-1][1]=B[rows-1].y-B[0].y;

	// Invert A0:
	transMatrix(A0,At,rows,cols); // A'
	mulMatrix(At,A0,Aquad,cols,rows,cols); // Aq = A'A
	if(!invMatrix(Aquad,Da,cols,DBL_EPSILON)) // Ainvq = Aq^-1
		return 0;
	mulMatrix(Da,At,invA0,cols,cols,rows); // Ainv = Ainvq * A'
	mulMatrix(A0,invA0,Ea,rows,cols,rows); // Ea = AA*-I
	for(i=0;i<rows;i++)
		Ea[i][i]-=1;

	// Get full vM and gradient:
	Mk=(invA0[0][0]-invA0[0][N-1])*(invA0[0][0]-invA0[0][N-1])+(invA0[1][0]-invA0[1][N-1])*(invA0[1][0]-invA0[1][N-1]); // mass_0
	omega=(invA0[0][N-1]-invA0[0][0])*(invA0[0][k_1]-invA0[0][k])+(invA0[1][N-1]-invA0[1][0])*(invA0[1][k_1]-invA0[1][k]); 
	epsilon=Ea[0][k]-Ea[0][k_1]-Ea[N-1][k]+Ea[N-1][k_1];
	grad->x=(omega+epsilon*Da[0][0])*(invA0[0][N-1]-invA0[0][0])+epsilon*Da[0][1]*(invA0[1][N-1]-invA0[1][0]);
	grad->y=epsilon*Da[0][1]*(invA0[0][N-1]-invA0[0][0])+(omega+epsilon*Da[1][1])*(invA0[1][N-1]-invA0[1][0]);
	for(i=1;i<N;i++)
	{
		Mk+=(invA0[0][i]-invA0[0][i-1])*(invA0[0][i]-invA0[0][i-1])+(invA0[1][i]-invA0[1][i-1])*(invA0[1][i]-invA0[1][i-1]); // mass_i
		omega=(invA0[0][i-1]-invA0[0][i])*(invA0[0][k_1]-invA0[0][k])+(invA0[1][i-1]-invA0[1][i])*(invA0[1][k_1]-invA0[1][k]);
		epsilon=Ea[i][k]-Ea[i][k_1]-Ea[i-1][k]+Ea[i-1][k_1];
		grad->x+=(omega+epsilon*Da[0][0])*(invA0[0][i-1]-invA0[0][i])+epsilon*Da[0][1]*(invA0[1][i-1]-invA0[1][i]);
		grad->y+=epsilon*Da[0][1]*(invA0[0][i-1]-invA0[0][i])+(omega+epsilon*Da[1][1])*(invA0[1][i-1]-invA0[1][i]);
	}
	grad->x*=2;
	grad->y*=2;
	return Mk;
}
double Trilat::getMassMax(int beacon, Point * start, BOOL routing)
{
	BOOL grow,buildRoute;
	Point *pos,grad,pos0,dir;
	IterInfo iter;
	double Mk;
	int n,n0;

	if(beacon<0)
		beacon=active;
	if(!start)
		start=p+POS_ERRMIN;
	pos=B+beacon;
	pos0=*pos;
	pos->copy(*start); // Iter start is the error minimum
	if(!(Mk=getMassGrad(&grad,beacon)))
		return 0;
	if(buildRoute=(routing&&bRoute.checked))
	{
		p[POS_START]=*pos;
		route.destroy();
	}
	iter.pos=*pos;
	iter.grad.copy(grad);
	iter.error=grad.dist();
	double coef=INV(iter.error);
	dir.x=coef*iter.grad.x;
	dir.y=coef*iter.grad.y;
	n=n0=1+__int(INV(stepReduct));
	iter.step=stepStart=(maxStepMass>0)?maxStepMass:minIterStep;
	maxStepMass=0;
	itersMmax=0;
	itersMin=1-__int(log(stepStart/minIterStep)/log(stepReduct));
	iter.func=Mk;
	iter.route=0;
	iter.count=0;
	grow=FALSE;
	while((iter.step>=minIterStep||iter.error>minIterGrad)&&itersMmax<maxIterCount)
	{
		itersMmax++;
		pos->x=iter.pos.x+iter.step*dir.x;
		pos->y=iter.pos.y+iter.step*dir.y;
		if(!(Mk=getMassGrad(&grad,beacon)))
			return 0;
		if(iter.func<Mk)
		{
			iter.func=Mk;
			iter.count++;
			if(maxStepMass<iter.step)
				maxStepMass=iter.step;
			iter.route+=iter.step;
			iter.pos.copy(*pos);
			iter.grad.copy(grad);
			iter.error=grad.dist();
			double coef=INV(iter.error);
			dir.x=coef*iter.grad.x;
			dir.y=coef*iter.grad.y;
			if(buildRoute)
				route.add(iter);
			if(n<n0)
				n++;
			else
				iter.step*=stepGrow;
		}
		else
		{
			n=0;
			iter.step*=stepReduct;
		}
	}
	if(buildRoute)
	{
		if(route.length&&graph==GRAPH_ROUTE)
		{
			IterObject iterObj;
			setGraph(GRAPH_ROUTE,route.length);
			for(iterObj=route.first;iterObj;iterObj=iterObj->next)
			{
				ADDVAL(CHNL_ITER_ROUTE,		iterObj->value.route);
				ADDVAL(CHNL_ITER_COUNT,		iterObj->value.count);
				ADDVAL(CHNL_ITER_STEP,		iterObj->value.step);
				//ADDVAL(CHNL_ITER_ERROR,	iterObj->value.error);
				//ADDVAL(CHNL_ITER_VAR,		iterObj->value.var);
				ADDVAL(CHNL_ITER_FUNC,		iterObj->value.func);
				ADDVAL(CHNL_ITER_GRAD,		iterObj->value.error);
				ADDVAL(CHNL_ITER_GRAD_X,	iterObj->value.grad.x);
				ADDVAL(CHNL_ITER_GRAD_Y,	iterObj->value.grad.y);
			}
			fg.graph.update();
		}
		p[POS_END]=*pos;
	}
	iterMass=iter;
	// return to the initial configuration
	pos->copy(pos0);
	//prefetch();
	return iterMass.func;
}
void Trilat::capture()
{
	Image screen,mapImage,graphImage;
	int quality,ext;
	const Rect<float> graphRect=fg.graph.getRect();
	quality=format.toInt(tbCompr.text,90);
	ext=chFormat.getSelectedIndex();
	File file;
	String name=tbCapture.text,path;

	Directory::setCurrent(currentDir);
#ifdef CAPTURE_DIR
	_tmkdir(CAPTURE_DIR);
	path=CAPTURE_DIR+('\\'+name);
#else
	path=name;
#endif
	if(mode3D)
	{
		screenshot(&screen);
		switch(ext)
		{
		case 1:
			writeBitmap(path+_T("_3D.bmp"),&screen);
			break;
		case 2:
			writeTGA(path+_T("_3D.tga"),&screen);
			break;
		default:
			writeJPEG(path+_T("_3D.jpg"),&screen,quality);
			break;
		}
	}
	else
	{
		screenshot(&mapImage,imgRect);
		screenshot(	&graphImage,
					__int(graphRect.x)+1,__int(graphRect.y),
					__int(graphRect.width)-1,__int(graphRect.height)-1);
		switch(ext)
		{
		case 1:
			writeBitmap(path+_T("_map.bmp"),&mapImage);
			writeBitmap(path+_T("_graph.bmp"),&graphImage);
			break;
		case 2:
			writeTGA(path+_T("_map.tga"),&mapImage);
			writeTGA(path+_T("_graph.tga"),&graphImage);
			break;
		default:
			writeJPEG(path+_T("_map.jpg"),&mapImage,quality);
			writeJPEG(path+_T("_graph.jpg"),&graphImage,quality);
			break;
		}
		if(file.open(path+_T(".plt"),_T("wb")))
			fg.graph.save(file,FALSE,' '+name);
	}
	dump(path+_T("_dump.txt"));
}
void Trilat::updateSectionMark()
{
	int index=fg.graph.markIndex;
	if(graph!=GRAPH_SECT||index>=points)
		return;
	mark.x=sectStart.x+index*(sectEnd.x-sectStart.x)/(points-1);
	mark.y=sectStart.y+index*(sectEnd.y-sectStart.y)/(points-1);
}
void Trilat::updateSection()
{
	double x,y,dx,dy,d,li=0,l,level,r,*lsm,*sect_arg,*values;
	int i;

	if(!mapSection)
		return;

	switch(section)
	{
	case SECT_R_MAPMIN:
		sectStart.copy(R);
		sectEnd.copy(p[POS_LEVMIN]);
		break;
	case SECT_R_MAPMAX:
		sectStart.copy(R);
		sectEnd.copy(p[POS_LEVMAX]);
		break;
	case SECT_R_VARMIN:
		sectStart.copy(R);
		sectEnd.copy(p[POS_ERRMIN]);
		break;
	case SECT_R_ACTIVE:
		sectStart.copy(R);
		sectEnd.copy(B[active]);
		break;
	case SECT_MIN_MAX:
		sectStart.copy(p[POS_LEVMIN]);
		sectEnd.copy(p[POS_LEVMAX]);
		break;
	case SECT_CUSTOM:
		sectStart.copy(custStart);
		sectEnd.copy(custEnd);
		break;
	default:
		return;
	}
	x=sectStart.x;
	y=sectStart.y;
	dx=sectEnd.x-sectStart.x;
	dy=sectEnd.y-sectStart.y;
	l=getLength(dx,dy);
	r=INV(points-1);

	dx*=r;
	dy*=r;
	d=l*r;

	values=buff[CHNL_SECT_VAL];
	ClearArray(values,points);
	if(!mapType)
		return;

	HANDLE current_thread = active_thread;
	sect_arg=buff[CHNL_SECT_ARG];
	for(i=0;i<points-1;i++,li+=d,x+=dx,y+=dy)
	{
		if(current_thread!=active_thread)return;
		sect_arg[i]=li;
		if(!getLevel(x,y,level))continue;
		values[i]=level;
	}
	if(current_thread!=active_thread)return;
	sect_arg[points-1]=l;
	if(getLevel(sectEnd.x,sectEnd.y,level))
		values[points-1]=level;
	
	lsm=buff[CHNL_SECT_LSM];
	if(getLSMPolyCoef(vSectCoefs.elems,vSectCoefs.size,values,sect_arg,points))
		for(i=0;i<points;i++)
			lsm[i]=funcValue(sect_arg[i],vSectCoefs.elems,vSectCoefs.size);
	else
		ClearArray(lsm,points);
	fg.graph.update();
	updateActiveInfo();
	updateSectionMark();
}
void Trilat::clearCust()
{
	trajectory.destroy();
	for(int i=CHNL_CUST_ARG;i<CHNL_ITER_COUNT;i++)
		channels[i]->clear();
}
void Trilat::addCust()
{
	double x,y,v;
	trajectory.add(R);
	ADDVAL(CHNL_CUST_ARG,			useCustArg?custArg:trajectory.length);
	ADDVAL(CHNL_CUST_STDEV_EST_X,	estStat.stdev.x);
	ADDVAL(CHNL_CUST_STDEV_EST_Y,	estStat.stdev.y);
	ADDVAL(CHNL_CUST_STDEV_EST,		estStat.stdev.dist());
	ADDVAL(CHNL_CUST_CORREL_EST,	estStat.correl);
	ADDVAL(CHNL_CUST_STDEV_NUM_X,	numStat.stdev.x);
	ADDVAL(CHNL_CUST_STDEV_NUM_Y,	numStat.stdev.y);
	ADDVAL(CHNL_CUST_STDEV_NUM,		numStat.stdev.dist());
	ADDVAL(CHNL_CUST_CORREL_NUM,	numStat.correl);

	x=estStat.stdev.x-numStat.stdev.x;
	y=estStat.stdev.y-numStat.stdev.y;
	ADDVAL(CHNL_CUST_STDEV_DIF_X,	x);
	ADDVAL(CHNL_CUST_STDEV_DIF_Y,	y);
	ADDVAL(CHNL_CUST_STDEV_DIF,		getLength(x,y));

	ADDVAL(CHNL_CUST_CORREL_DIF,	estStat.correl-numStat.correl);

	v=getParabVar(R);
	if(v)
		ADDVAL(CHNL_CUST_VAR_PAR_DIF,	estStat.stdev.square()-v);

	ADDVAL(CHNL_CUST_MEAN_X,		numStat.mean.x);
	ADDVAL(CHNL_CUST_MEAN_Y,		numStat.mean.y);
	ADDVAL(CHNL_CUST_MEAN,			numStat.mean.dist());
	ADDVAL(CHNL_CUST_EXCESS_X,		numStat.excess.x);
	ADDVAL(CHNL_CUST_EXCESS_Y,		numStat.excess.y);
	ADDVAL(CHNL_CUST_EXCESS,		numStat.excess.dist());
	ADDVAL(CHNL_CUST_SKEW_X,		numStat.skew.x);
	ADDVAL(CHNL_CUST_SKEW_Y,		numStat.skew.y);
	ADDVAL(CHNL_CUST_SKEW,			numStat.skew.dist());
	ADDVAL(CHNL_CUST_MASS,			1e3*M[active]);
	ADDVAL(CHNL_CUST_FULL_MASS,		FGW);
	ADDVAL(CHNL_CUST_MIN_VAR,		minVar.x+minVar.y);
	ADDVAL(CHNL_CUST_AVG_RAD,		detU);
	ADDVAL(CHNL_CUST_UX,			U[0][0]);
	ADDVAL(CHNL_CUST_UY,			U[1][0]);
	ADDVAL(CHNL_CUST_UXY,			U[2][0]);
	ADDVAL(CHNL_CUST_LEV_MIN,		levmin);
	ADDVAL(CHNL_CUST_LEV_MAX,		levmax);
	ADDVAL(CHNL_CUST_LEV_MID,		levmid);
	ADDVAL(CHNL_CUST_TIME,			recalc_time);
}
BOOL Trilat::listen(WORD id, WORD code, const void *info)
{
	Lock lock1(&draw_sect);

	if(	id==bArray.id||
		id==tbCount.id||
		id==tbSamples.id||
		id==bKeepErrors.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			stop(FALSE);
			setArray(	format.toInt(tbCount.text,0),
						format.toInt(tbSamples.text,0),
						bKeepErrors.checked);
			updateStats();
			startMap();
		}
		return TRUE;
	}
	if(id==chModel.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			stop(FALSE);
			setModel(chModel.getSelectedIndex());
			prefetch();
			startMap();
		}
		return TRUE;
	}
	else if(id==bWinSize.id||id==tbWinSizeX.id||id==tbWinSizeY.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			resizeWindow(tbWinSizeX.getInt(),tbWinSizeY.getInt());
		}
		return TRUE;
	}
	else if(id==bImage.id||
			id==tbArea.id||
			id==tbImage.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			stop(FALSE);
			setImage(	format.toInt(tbImage.text,0),
						format.toInt(tbArea.text,0));
			prefetch();
			startMap();
		}
		return TRUE;
	}
	else if(id==fg.id)
	{
		if(code==FLAT_ACTION_SELECT)
		{
			if(info!=NULL&&iterIndex!=fg.graph.markIndex)
			{
				start(ACTION_GRAPH);
			}
		}
		return TRUE;
	}
	else if(id==bStat.id||
			id==tbStdevXY.id||
			id==tbCorrXY.id||
			id==tbStdevL.id||
			id==tbMeanL.id||
			id==bRandev.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			stop(FALSE);
			setStat(	format.toDouble(tbStdevXY.text,0),
						format.toDouble(tbCorrXY.text,0),
						format.toDouble(tbStdevL.text,0),
						format.toDouble(tbMeanL.text,0),
						bRandev.checked);
			buildStat();
			updateStats();
			startMap();
		}
		return TRUE;
	}
	else if(id==chOpt.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			stop(FALSE);
			setOptim(chOpt.getSelectedIndex());
			startMap();
		}
		return TRUE;
	}
	else if(id==bShow3D.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			set3D(TRUE);
		}
		return TRUE;
	}
	else if(id==chOrder.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			stop(FALSE);
			setOrder(chOrder.getSelectedIndex());
			prefetch();
			startMap();
		}
		return TRUE;
	}
	else if(id==bCapture.id||
			id==tbCapture.id||
			id==tbCompr.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			capture();
		}
		return TRUE;
	}
	else if(id==bIter.id||
			id==tbMinIterStep.id||
			id==tbMaxIterCount.id||
			id==tbMinIterGrad.id||
			id==tbStepReduct.id||
			id==tbStepGrow.id||
			id==tbErrDist.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			stop(FALSE);
			setIterParams(	format.toDouble(tbMinIterStep.text),
							format.toInt(tbMaxIterCount.text),
							format.toDouble(tbMinIterGrad.text),
							format.toDouble(tbStepReduct.text,DEF_STEP_REDUCT),
							format.toDouble(tbStepGrow.text,DEF_STEP_GROW),
							format.toDouble(tbErrDist.text));
			startMap();
		}
		return TRUE;
	}
	else if(id==chMap.id||
			id==chVar.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			stop(FALSE);
			setMap(chMap.getSelectedIndex(),chVar.getSelectedIndex());
			startMap();
		}
		return TRUE;
	}
	else if(id==chGraph.id||
			id==tbPoints.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			stop(FALSE);
			setGraph(	chGraph.getSelectedIndex(),
						format.toInt(tbPoints.text,100));
			fg.graph.update();
		}
		return TRUE;
	}
	else if(id==bThreshold.id||
			id==tbInf.id||
			id==tbSup.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			setThreshold(	format.toDouble(tbInf.text),
							format.toDouble(tbSup.text));
			startMap();

		}
		return TRUE;
	}
	else if(id==chSection.id||
			id==bLSM.id||
			id==tbLSM.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			stop(FALSE);
			setSection(	chSection.getSelectedIndex(),
						format.toInt(tbLSM.text,1));
		}
		return TRUE;
	}
	else if(id==bScript.id||
			id==chScript.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			if(pending_action!=ACTION_SCRIPT)
			{
				pending_action=ACTION_SCRIPT;
				stop(FALSE);
				start(ACTION_SCRIPT);
			}
		}
		return TRUE;
	}
	else if(id==bAddCust.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			addCust();
			fg.graph.update();
		}
		return TRUE;
	}
	else if(id==bFitCust.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			stop(FALSE);
			start(ACTION_FIT);
		}
		return TRUE;
	}
	else if(id==bClearCust.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			stop(FALSE);
			clearCust();
		}
		return TRUE;
	}
	else if(id==bDump.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			Directory::setCurrent(currentDir);
			dump(tbDump.text);
		}
	}
	else if(id==chActive.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			stop(FALSE);
			setActiveBeacon(chActive.getSelectedIndex());
		}
		return TRUE;
	}
	else if(id==bGoTo.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			if(cog_ok)
			{
				R.copy(p[POS_ERRMIN]);
				showPos();
				if(mapArg!=MAP_ARG_POINT||mapNoArg)
				{
					startMap();
				}
				else if(!processing)
				{
					updateSection();
					recalc(bAllCust.checked);
				}
			}
		}
		return TRUE;
	}
	else if(id==bParabEstim.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			stop(FALSE);
			allowParabEstim(bParabEstim.checked);
			startMap();
		}
		return TRUE;
	}
	else if(id==bActiveUpdate.id||
			id==tbActiveX.id||
			id==tbActiveY.id||
			id==tbActiveStdevX.id||
			id==tbActiveStdevY.id||
			id==tbActiveStdevL.id||
			id==tbActiveMeanL.id||
			id==tbActiveCorrel.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			stop(FALSE);
			setActiveInfo(	format.toDouble(tbActiveX.text),
							format.toDouble(tbActiveY.text),
							format.toDouble(tbActiveStdevX.text),
							format.toDouble(tbActiveStdevY.text),
							format.toDouble(tbActiveCorrel.text),
							format.toDouble(tbActiveStdevL.text),
							format.toDouble(tbActiveMeanL.text));
			startMap();
		}
		return TRUE;
	}
	else if(id==bActiveSwap.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			stop(FALSE);
			swap(active);
			startMap();
		}
		return TRUE;
	}
	else if(id==bMapLims.id||
			id==tbMapMin.id||
			id==tbMapMax.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			setMapLims(	format.toDouble(tbMapMin.text),
						format.toDouble(tbMapMax.text));
			if(	id==tbMapMin.id||
				id==tbMapMax.id)
				bMapLims.checked=TRUE;
			startMap();
		}
		return TRUE;
	}
	else if(id==bIterStart.id||
			id==tbIterStartX.id||
			id==tbIterStartY.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			p[POS_START].set(	format.toDouble(tbIterStartX.text),
								format.toDouble(tbIterStartY.text));
			showPos();
			startMap();
		}
		return TRUE;
	}
	else if(id==bStartToR.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			double x=format.toDouble(tbIterStartX.text);
			double y=format.toDouble(tbIterStartY.text);
			p[POS_START].set(x,y);
			R.set(x,y);
			showPos();
			startMap();
		}
		return TRUE;
	}
	else if(id==bRtoStart.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			double x=format.toDouble(tbRx.text);
			double y=format.toDouble(tbRy.text);
			p[POS_START].set(x,y);
			R.set(x,y);
			showPos();
			startMap();
		}
		return TRUE;
	}
	else if(id==bR.id||
			id==tbRx.id||
			id==tbRy.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			R.set(	format.toDouble(tbRx.text),
					format.toDouble(tbRy.text));
			showPos();
			startMap();
		}
		return TRUE;
	}
	else if(id==bLoad.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			stop(FALSE);
			CSTR filename=tbFile.text;
			if(bBrowse.checked||!filename||!filename[0])
				filename=fd.open();
			if(filename)
				load(filename);
		}
		return TRUE;
	}
	else if(id==bSave.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			CSTR filename=tbFile.text;
			if(bBrowse.checked||!filename||!filename[0])
				filename=fd.save(filename);
			if(filename)
				save(filename);
		}
		return TRUE;
	}
	else if(id==bMotionEstim.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			allowMotionEstim(bMotionEstim.checked);
			startMap();
		}
		return TRUE;
	}
	else if(id==bMassMax.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			if(!processing&&model==MODEL_S_TOF)
				getMassMax(active,p+POS_START,TRUE);
		}
		return TRUE;
	}
	else if(id==bConsole.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			if(bConsole.checked)
				console.open(window.title);
			else
				console.close();
		}
	}
	else if(id==bPlan.id||
			id==chPlan.id||
			id==tbTargetX.id||
			id==tbTargetY.id||
			id==tbMotionEqual.id||
			id==tbMotionCritic.id||
			id==tbMotionActive.id||
			id==bForceM.id||
			id==bForceP.id||
			id==bForceC.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			stop(FALSE);
			setMotionTarget(format.toDouble(tbTargetX.text,DEF_TARGET_X),
							format.toDouble(tbTargetY.text,DEF_TARGET_Y));
			setMotionParams(format.toDouble(tbMotionEqual.text,DEF_EQUAL_REGION),
							format.toDouble(tbMotionCritic.text,DEF_CRITIC_REGION),
							format.toDouble(tbMotionActive.text,DEF_ACTIVE_REGION),
							format.toDouble(tbSpeedReduct.text,DEF_SPEED_REDUCT));
			start(ACTION_PLAN);
		}
		return TRUE;
	}
	return FALSE;
}
BOOL Trilat::input(BYTE type)
{
	double x,y;
	if(FlatApplication::input(type))
		return TRUE;
	Lock lock(&draw_sect);
	if(mode3D)
	{
		switch(type)
		{
			case MOUSE_INPUT_MSG:
			{
				switch(msg.message)
				{
					case WM_RBUTTONDOWN:
					{
						set3D(FALSE);
					}
					break;
					case WM_MOUSEMOVE:
					{
						if(mouse.lbutton)
						{
							angleZ+=(360.0f*(mouse.x-oldMousePos.x))/window.width;
							angleX+=(360.0f*(mouse.y-oldMousePos.y))/window.height;
						}
						oldMousePos.set(mouse.x,mouse.y);
					}
					break;
					case WM_MOUSEWHEEL:
					{
						distZ+=0.1f*mouse.wheel;
						if(distZ<-screen.zFarPers)
							distZ=float(-screen.zFarPers);
						else if(distZ>-screen.zNearPers)
							distZ=float(-screen.zNearPers);
					}
					break;
				}
			}
			break;
			case KEY_INPUT_MSG:
			{
				if(keyboard.pressed)
				{
					if(keyboard.waiting) switch(keyboard.code)
					{
						case VK_F2:
						case VK_ESCAPE:
							set3D(FALSE);
							break;
						case VK_F6:
							capture();
							break;
						case VK_UP:
							angleX--;
							break;
						case VK_DOWN:
							angleX++;
							break;
						case VK_LEFT:
							angleZ--;
							break;
						case VK_RIGHT:
							angleZ++;
							break;
						case VK_PRIOR:
							distZ-=0.1f;
							if(distZ<-screen.zFarPers)
								distZ=float(-screen.zFarPers);
							break;
						case VK_NEXT:
							distZ+=0.1f;
							if(distZ>-screen.zNearPers)
								distZ=float(-screen.zNearPers);
							break;
						case VK_SPACE:
							init3D();
							get3DWebStep(TRUE);
							break;
						default:
							return FALSE;
					}
					else switch(keyboard.symbol)
					{
						case 's':
							smooth3D=!smooth3D;
							break;
						case 'l':
							light3D=!light3D;
							break;
						case 'f':
							frame3D=!frame3D;
							break;
						case 'b':
							box3D=!box3D;
							break;
						case 't':
							xyz3D=!xyz3D;
							break;
						case 'v':
							values3D=!values3D;
							break;
						case 'w':
							web3D=!web3D;
							break;
						case 'o':
							onlyWeb=!onlyWeb;
							break;
						case 'c':
							color3D=!color3D;
							compile3D();
							break;
						case 'i':
							info3D=!info3D;
							break;
						case 'm':
							minMax3D=!minMax3D;
							break;
						case '=':
						case '+':
							count3DWeb++;
							get3DWebStep(FALSE);
							break;
						case '-':
						case '_':
							count3DWeb--;
							get3DWebStep(TRUE);
							break;
						default:
							return FALSE;
					}
					return TRUE;
				}
			}
		}
	}
	else
	{
		switch(type)
		{
		case MOUSE_INPUT_MSG:
			if(imgRect.contain(mouse.x,mouse.y))
			{
				BOOL drawMap=FALSE;
				x=DEMAPX(mouse.x);
				y=DEMAPY(mouse.y);
				if(mouse.lbutton)
				{
					if(mouse.ctrl)
					{
						if(section==SECT_CUSTOM)
							custStart.set(x,y);
						else
						{
							p[POS_ITER].set(x,y);
							drawMap=(mapArg==MAP_ARG_POINT&&map==MAP_MLK);
						}
					}
					else if(mouse.shift)
					{
						p[POS_START].set(x,y);
						drawMap=(mapArg!=MAP_ARG_START&&model>=MODEL_ITER&&mapType!=MAP_TYPE_ENERGY);
						showPos();
					}
					else
					{
						R.set(x,y);
						drawMap=mapNoArg||((mapArg!=MAP_ARG_POINT)&&(map<MAP_MASS||map>MAP_GRAD_MASS));
						showPos();
					}
				}
				else if(mouse.rbutton)
				{
					if(mouse.ctrl&&section==SECT_CUSTOM)
					{
						custEnd.set(x,y);
					}
					else if(mouse.shift)
					{
						setMotionTarget(x,y);
					}
					else
					{
						stop();
						drawMap=(mapArg!=MAP_ARG_BEACON);
						if(msg.message==WM_RBUTTONDOWN)
						{
							double dist,distMin=B[0].dist(x,y);
							int indMin=0;
							for(int i=1;i<N;i++)
							{
								dist=B[i].dist(x,y);
								if(distMin>dist)
								{
									distMin=dist;
									indMin=i;
								}
							}
							if(setActiveBeacon(indMin))
								drawMap=TRUE;
						}
						tbActiveX.setDouble(x);
						tbActiveY.setDouble(y);
						B[active].set(x,y);
						prefetch();
					}
				}
				else
				{
					return FALSE;
				}
				if(drawMap)
				{
					//PRINT(_T("MAP\n"));
					startMap();
				}
				else if(!processing)
				{
					updateSection();
					recalc(bAllCust.checked);
				}
				return TRUE;
			}
			break;
		case KEY_INPUT_MSG:
			if(keyboard.pressed)
			{
				if(keyboard.waiting)
				{
					switch(keyboard.code)
					{
						//case VK_F2:
						//	set3D(TRUE);
						//	return TRUE;
						case VK_UP:
							R.y+=INV(zoom);
							break;
						case VK_DOWN:
							R.y-=INV(zoom);
							break;
						case VK_LEFT:
							R.x-=INV(zoom);
							break;
						case VK_RIGHT:
							R.x+=INV(zoom);
							break;
						default:
							return FALSE;
					}
					showPos();
				}
				else switch(keyboard.symbol)
				{
					case '1':
					case '2':
					case '3':
					case '4':
					case '5':
					case '6':
					case '7':
					case '8':
					case '9':
						setActiveBeacon(keyboard.symbol-'1');
						startMap();
						return TRUE;
					default:
						return FALSE;
				}
				if(mapArg!=MAP_ARG_POINT||mapNoArg)
				{
					startMap();
				}
				else if(!processing)
				{
					updateSection();
					recalc(bAllCust.checked);
				}
				return TRUE;
			}
		}
	}
	return FALSE;
}
void Trilat::dump(CSTR filename)
{
	if(console.fOutput)
	{
		dump(console.fOutput);
	}
	else if(filename&&filename[0])
	{
		FILE *file=_tfopen(filename,_T("wt"));
		dump(file);
		fclose(file);
	}
}
#define DUMP(format,...) _ftprintf(file, format,__VA_ARGS__)
#define DUMPM(matrix,rows,cols,...) printMatrix(file,matrix,rows,cols,__VA_ARGS__)
BOOL Trilat::dump(FILE *file)
{
	BOOL result=FALSE;
	double normReal,normEst;
	int i;
	Point out;
	StatBeacon *sb;

	if(file)
	{
		result=TRUE;
		normReal=numStat.stdev.dist();
		normEst=estStat.stdev.dist();
		DUMP(_T("\n*** TRIANG DUMP INFO ***\n"));

		DUMP(_T("\n\nCONFIG________________________________________________________________\n\n"));
		DUMP(_T("beacons: %10d,  samples: %10d\n"),N,K);
		DUMP(_T("stdevXY: %.3le,   stdevL: %.3le\n"),stdevXY,stdevL);
		DUMP(_T("  model: %10d,     area: %10d\n"),model,areaSide);

		double dx=estStat.stdev.x-numStat.stdev.x;
		double dy=estStat.stdev.y-numStat.stdev.y;

		DUMP(_T("\n\nRESULTS_______________________________________________________________\n\n"));
		DUMP(_T("    POINT: (% f, % f)\n"),R.x,R.y);
		DUMP(_T("   EXCESS: (% le, % le), mod=%le\n"),numStat.excess.x,numStat.excess.y,numStat.excess.dist());
		DUMP(_T("     SKEW: (% le, % le), mod=%le\n"),numStat.skew.x,numStat.skew.y,numStat.skew.dist());
		DUMP(_T("     MEAN: (% le, % le), mod=%le\n"),numStat.mean.x,numStat.mean.y,numStat.mean.dist());
		DUMP(_T("NUM_STDEV: (% le, % le), mod=%le, cor=% le\n"),numStat.stdev.x,numStat.stdev.y,normReal,numStat.correl);
		DUMP(_T("EST_STDEV: (% le, % le), mod=%le, cor=% le\n"),estStat.stdev.x,estStat.stdev.y,normEst,estStat.correl);
		DUMP(_T("ERR_STDEV: x=% le(%le%%), y=% le(%le%%), mod=%le, cor=% le\n"),
			dx,absRelErr(estStat.stdev.x,numStat.stdev.x),
			dy,absRelErr(estStat.stdev.y,numStat.stdev.y),
			getLength(dx,dy),
			estStat.correl-numStat.correl);

		DUMP(_T("\n\nMAP_______________________________________________________________\n\n"));
		DUMP(_T("     NAME: %s\n"),nameMaps[map]);
		DUMP(_T("      MIN: % le, X: % le, Y: % le\n"),levmin,p[POS_LEVMIN].x,p[POS_LEVMIN].y);
		DUMP(_T("      MAX: % le, X: % le, Y: % le\n"),levmax,p[POS_LEVMAX].x,p[POS_LEVMAX].y);
		DUMP(_T("      MID: % le\n"),levmid);
			
		DUMP(_T("\n\nMATRICES_______________________________________________________________\n"));
		DUMPM(A,rows,cols,_T("\nA=\n"),_T("\n"),_T("% .1le"));
		DUMPM(D,rows,1,_T("\nB=\n"),_T("\n"),_T("% .1le"));
		DUMPM(X,cols,1,_T("\nR=\n"),_T("\n"),_T("% .1le"));
		DUMPM(Aquad,cols,cols,_T("\nA'A=\n"),_T("\n"),_T("% .1le"));
		DUMP(_T("\nsqrtdet=%e\n"),detfactor);
		DUMPM(Da,cols,cols,_T("\ninv(A'A)=\n"),_T("\n"),_T("% .1le"));
		DUMPM(invA,cols,rows,_T("\nA*=\n"),_T("\n"),_T("% .1le"));
		DUMPM(Ea,rows,rows,_T("\nEa*=\n"),_T("\n"),_T("% .1le"));

		DUMP(_T("\n\nBEACONS________________________________________________________________\n"));

		for(i=0;i<N;i++)
		{
			sb=&stats[i];
			DUMP(_T("\n\nBeacon %d:\n"),i+1);
			DUMP(_T("pos=(% .3f,% .3f), dist=%f\n"),B[i].x,B[i].y,Lr[i]);
			DUMP(_T("devx=%f, devy=%f, corr=% f, devl=%f\n"),sb->devx,sb->devy,sb->corxy,sb->devl);
			DUMP(_T("optim=%le\n"),O[i]);
			DUMP(_T("trans=% le, m=% le, gradm=% le\n"),Lr[i]*sqrt(2*M[i]),M[i],gradM[i].dist());
			DUMPM(mJ[i],2,3,_T("Jacobian:\n"),_T("\n"),_T("% .3le"));
		}

		DUMP(_T("\n\nSECTION________________________________________________________________\n\n"));
		DUMP(_T("\nlength: %le\n"),getLength(sectEnd.x-sectStart.x,sectEnd.y-sectStart.y));
		DUMP(_T("\nLSM polynomial coefficients:\n"));
		for(i=0; i<vSectCoefs.size; i++)
			DUMP(_T("coef[%d]=% le\n"),i,vSectCoefs[i]);

		DUMP(_T("\n\nMASS INFO_____________________________________________________________\n"));
		if(cog_ok)
		{
			DUMP(_T("\nFGW: %le\n"),FGW);
			DUMP(_T("Rmin: x=% le, y=% le\n"),p[POS_ERRMIN].x,p[POS_ERRMIN].y);
			if(parabOK)
			{
				DUMP(_T("\nParabola analyse:\n"));
				DUMP(_T("* Min Var: % le (stdev=% le)\n"),minVar.x+minVar.y,sqrt(minVar.x+minVar.y));
				DUMP(_T("* U det: % le\n"),detU);
				DUMP(_T("* Uxx: % le, Uyy: % le, Uxy: %le\n"),U[0][0],U[1][0],U[2][0]);
			}
			if(itersMmax&&iterMass.count)
			{
				DUMP(_T("\nLast computed max full vM:\n"));
				DUMP(_T("* max full vM: x=% le, y=% le, val=%le\n"),iterMass.pos.x,iterMass.pos.y,iterMass.func);
				DUMP(_T("* gradient vM: x=% le, y=% le, mod=%le\n"),iterMass.grad.x,iterMass.grad.y,iterMass.error);
				DUMP(_T("* iterations: ALL=%d, HIT=%d, MIN=%d, BAD=%d\n"),itersMmax,iterMass.count,itersMin,itersMmax-iterMass.count-itersMin);
				DUMP(_T("* succesful step: max=%le, min=%le, avg=%le\n"),maxStepMass,iterMass.step,iterMass.route/iterMass.count);
				DUMP(_T("* parameter step: start=%le, reduct=%le, grow=%le\n"),stepStart, stepReduct, stepGrow);
				DUMP(_T("* traversed distnace: %le\n"),iterMass.route);
			}
			if(iterMotion.count)
			{
				DUMP(_T("\nLast computed motion plan:\n"));
				DUMP(_T("* params: lt=%le, la=%le, rc=%le\n"),lt,la,rc);
				DUMP(_T("* point: x=% le, y=% le,\n"),iterMotion.pos.x,iterMotion.pos.y);
				DUMP(_T("* route=%le, error=%le, step=%le\n"),iterMotion.route,iterMotion.error,iterMotion.step);
				DUMP(_T("* force: x=% le, y=% le, mod=%le\n"),iterMotion.grad.x,iterMotion.grad.y,iterMotion.grad.dist());
				DUMP(_T("* fgw: %le\n"),iterMotion.func);
				DUMP(_T("* var: %le\n"),iterMotion.var);
				DUMP(_T("* iterations: %d\n"),iterMotion.count); 
			}
		}

		DUMP(_T("\n\nPOSITION INFO_____________________________________________________________\n"));
		for(i=0;i<POS_COUNT;i++)
			DUMP(_T("%s: (% le, % le)\n"),nameShow[i],p[i].x,p[i].y);

		DUMP(_T("\n\nITER INFO_____________________________________________________________\n"));
		DUMP(_T("count: %d\n"),iterOut.count);
		DUMP(_T("wrong: %.3f%%\n"),iterErrRatio*100);
		DUMP(_T("pos:   (%+le,%+le)\n"),iterOut.pos.x,iterOut.pos.y);
		DUMP(_T("error: %le\n"),iterOut.error);
		DUMP(_T("step:  %le\n"),iterOut.step);
		DUMP(_T("func:  %le\n"),iterOut.func);
		DUMP(_T("grad:  (%+le,%+le) %le\n"),iterOut.grad.x,iterOut.grad.y,iterOut.grad.dist());
		DUMP(_T("route: %le\n"),iterOut.route);
		DUMP(_T("trig:  %le\n"),iterOut.trig);
		DUMP(_T("var:   %le\n"),iterOut.var);
		
		DUMP(_T("\n\nMEMORY INFO_____________________________________________________________\n"));
		PRINTMEM();
	}
	return result;
}
void Trilat::draw3D()
{
	int w,h;
	double pos3D[4][3];
	if(!mode3D)
		return;
	glTranslatef(distX,distY,distZ);
	glRotate(angleX,angleY,angleZ);

	trans.update();
	trans.getWindowCoor( 1.0,-1.0, 0.0,pos3D[AXE_X]);
	trans.getWindowCoor(-1.0, 1.0, 0.0,pos3D[AXE_Y]);
	trans.getWindowCoor(-1.0,-1.0, 1.0,pos3D[AXE_Z]);
	trans.getWindowCoor(-1.0,-1.0, 0.0,pos3D[AXE_W]);

	glPushAttrib(GL_ENABLE_BIT);
	glPushAttrib(GL_LINE_BIT);

	glEnable(GL_COLOR_MATERIAL);
	setDepthTest(TRUE,1,GL_LESS);
	setPolygonOffset(TRUE);
	setBlend(smooth3D);
	setSmooth(smooth3D,GL_NICEST);

	if(!(web3D&&onlyWeb))
	{
		setLight(light3D);
		list3D.execute();
	}
	setLight(FALSE);
	
	if(web3D)
	{
		int tmp;
		glLineWidth(1.0f);
		setColor(INT_BLACK);

		for(h=0;h<=count3DWeb;h++)
		{
			tmp=h*step3DWeb*areaSide;
			glBegin(GL_LINE_STRIP);
			for(w=0;w<areaSide;w++)
				glVertex3fv(buff3D[tmp+w].vertex);
			glEnd();
		}
		for(w=0;w<=count3DWeb;w++)
		{
			tmp=w*step3DWeb;
			glBegin(GL_LINE_STRIP);
			for(h=0;h<areaSide;h++)
				glVertex3fv(buff3D[h*areaSide+tmp].vertex);
			glEnd();
		}
	}
	if(frame3D)
	{
		// bold frame
		glLineWidth(3.0f);
		setColor(INT_BLACK);

		glBegin(GL_LINES);
		glVertex3i(-1,-1,0);
		glVertex3i(1,-1,0);
		glVertex3i(-1,-1,0);
		glVertex3i(-1,1,0);
		glVertex3i(-1,-1,1);
		glVertex3i(-1,-1,0);
		glEnd();

		if(box3D)
		{
			// thin box
			setColor(INT_GRAY);
			glLineWidth(1.5f);

			glBegin(GL_LINES);
			glVertex3i(-1,1,0);
			glVertex3i(-1,1,1);
			glVertex3i(1,1,0);
			glVertex3i(1,1,1);
			glVertex3i(1,-1,0);
			glVertex3i(1,-1,1);
			glVertex3i(1,1,0);
			glVertex3i(-1,1,0);
			glVertex3i(1,1,0);
			glVertex3i(1,-1,0);
			glVertex3i(1,1,1);
			glVertex3i(-1,1,1);
			glVertex3i(1,1,1);
			glVertex3i(1,-1,1);
			glVertex3i(-1,-1,1);
			glVertex3i(1,-1,1);
			glVertex3i(-1,-1,1);
			glVertex3i(-1,1,1);
			glEnd();
		}
		if(xyz3D)
		{
			fontBig.setPosition(1.08f,-1.08f,0.08f);
			fontBig.print(_T("X"));
			fontBig.setPosition(-1.08f,1.08f,0.08f);
			fontBig.print(_T("Y"));
			fontBig.setPosition(-1.08f,-1.08f,1.08f);
			fontBig.print(_T("Z"));
		}
	}
	glPopAttrib();
	glPopAttrib();
}
void Trilat::draw2D()
{
	CSTR text;
	int i,j,nScale;
	char cScale;
	float x,y,left,right,start,zoomG;
	double normReal,normEst,time;
	BOOL showErrors=bShowErrors.checked&&mapType!=MAP_TYPE_DISTRIB;

	if(mode3D)
	{
		if(info3D)
		{
			fontSmall.color=INT_DGRAY;
			fontSmall.x=10;
			fontSmall.y=abs(fontSmall.height)+LINE;
			activeFont=&fontSmall;
			GPRINT(_T("web: show(w)=%d, only(o)=%d, count(+/-)=%d, step=%d"),web3D,onlyWeb,count3DWeb,step3DWeb)
			GPRINT(_T("smooth(s)=%d, light(l)=%d, color(c)=%d, frame(f)=%d"),smooth3D,light3D,color3D,frame3D)
			GPRINT(_T("dx=%+.1lf; dy=%+.1lf; dz=%+.1lf"),distX,distY,distZ)
			GPRINT(_T("ax=%+4d; ay=%+4d; az=%+4d"),__int(angleX)%360,__int(angleY)%360,__int(angleZ)%360)
			GPRINT(_T("loop(ms)=%.1f"),loopTime*1e3)

			fontSmall.x=float(window.width)-230.0f;
			fontSmall.y=abs(fontSmall.height)+LINE;
			GPRINT(_T("MIN: %+.3le, x: %3.0lf, y: %3.0lf"),levmin,p[POS_LEVMIN].x,p[POS_LEVMIN].y)
			GPRINT(_T("MAX: %+.3le, x: %3.0lf, y: %3.0lf"),levmax,p[POS_LEVMAX].x,p[POS_LEVMAX].y)
			GPRINT(_T("MID: %+.3le"),levmid)
		}
		if(minMax3D)
		{
			fontBig.align=Font::ALIGN_CENTER;
			fontBig.color=INT_BLACK;
			fontBig.setPosition(float(0.5f*window.width),float(window.height-1.5f*abs(fontBig.height)));
			activeFont=&fontBig;
			GPRINT(_T("min: %+.3le, x: %4.1lf, y: %4.1lf"),levmin,p[POS_LEVMIN].x,p[POS_LEVMIN].y)
			GPRINT(_T("max: %+.3le, x: %4.1lf, y: %4.1lf"),levmax,p[POS_LEVMAX].x,p[POS_LEVMAX].y)
			activeFont=&fontSmall;
			fontBig.align=Font::ALIGN_LEFT;
		}
		return;
	}
	fontSmall.color=INT_BLACK;

	fontSmall.x=TEXT_START_X;
	fontSmall.y=TEXT_START_Y;

	GPRINT(_T("  MEAN: (%8.3lf, %8.3lf), mod=%7.3lf, la=%8.3lf"),
			numStat.mean.x,numStat.mean.y,numStat.mean.dist(),numStat.meanL)
	double skewModule=numStat.skew.dist();
	if(skewModule<10)
		GPRINT(_T("  SKEW: (%8.3lf, %8.3lf), mod=%7.3lf, la=%8.3lf"),
				numStat.skew.x,numStat.skew.y,skewModule,numStat.skewL)
	else
		GPRINT(_T("  SKEW: ERROR (%e)"), skewModule)
	double excessModule=numStat.excess.dist();
	if(excessModule<10)
		GPRINT(_T("EXCESS: (%8.3lf, %8.3lf), mod=%7.3lf, la=%8.3lf"),
				numStat.excess.x,numStat.excess.y,excessModule,numStat.excessL)
	else
		GPRINT(_T("EXCESS: ERROR (%e)"), excessModule)

	fontSmall.x=TEXT_START_X2;
	fontSmall.y=TEXT_START_Y;

	normReal=numStat.stdev.dist();
	normEst=estStat.stdev.dist();
	double dx=estStat.stdev.x-numStat.stdev.x;
	double dy=estStat.stdev.y-numStat.stdev.y;
	GPRINT(_T("NUM_STDEV: (%7.3lf, %7.3lf), mod=%7.3lf, cor=% 5.3lf, l=%7.3lf"),
			numStat.stdev.x,numStat.stdev.y,normReal,numStat.correl,numStat.stdevL)
	GPRINT(_T("EST_STDEV: (%7.3lf, %7.3lf), mod=%7.3lf, cor=% 5.3lf, l=%7.3lf"),
			estStat.stdev.x,estStat.stdev.y,normEst,estStat.correl,estStat.stdevL)
	GPRINT(_T("DIF_STDEV: (%7.3lf, %7.3lf), mod=%7.3lf, cor=% 5.3lf, l=%7.3lf"),
			dx,dy,getLength(dx,dy),estStat.correl-numStat.correl,estStat.stdevL-numStat.stdevL)

	fontSmall.x=TEXT_START_X3;
	fontSmall.y=TEXT_START_Y;
	switch(model)
	{
	case MODEL_S_TOF:
	case MODEL_TOF:
	case MODEL_S_TDOA:
	case MODEL_S_TDOA2:
	case MODEL_TDOA:
		GPRINT(_T("   FACT: %.3le"),detfactor)
		GPRINT(_T("    FGW: %.3le"),FGW)
		break;
	case MODEL_ITER_A:
	case MODEL_ITER_A2:
	case MODEL_ITER_G:
	case MODEL_ITER_Q:
		GPRINT(_T("  ITERS: %d"),iterOut.count)
		GPRINT(_T("   STEP: %.3le"),iterOut.step)
		GPRINT(_T("  ERROR: %.3le"),iterOut.error)
		break;
	}
	drawLineIndicator(	imgRect.x,imgRect.y+imgRect.height+5,
						imgRect.width,5,
						(float)progress,1,DIR_RIGHT,
						INT_BLUE,0,0,INT_WHITE,INT_BLACK);

	fontSmall.x=imgRect.x;
	fontSmall.y=imgRect.y+imgRect.height+25;
	time=processing?actionTimer.time():actionTime;
	if(time<1e-2)
	{
		nScale=1000000;
		cScale='u';
	}
	else if(time<10)
	{
		nScale=1000;
		cScale='m';
	}
	else
	{
		nScale=1;
		cScale=' ';
	}
	GPRINT(_T("POINT: (%8.3lf, %8.3lf)"),R.x,R.y)
	GPRINT(_T("PROGRESS: %3d%%, TIME: %6d%cs"),round(100*progress),__int(nScale*time),cScale)
	GPRINT(_T("MIN: %+le, (%3.0lf, %3.0lf)"),levmin,p[POS_LEVMIN].x,p[POS_LEVMIN].y)
	GPRINT(_T("MAX: %+le, (%3.0lf, %3.0lf)"),levmax,p[POS_LEVMAX].x,p[POS_LEVMAX].y)
	GPRINT(_T("MID: %+le"),levmid)
	GPRINT(_T("RESULT: %s (%d)"),nameResults[action_result],(action_result==ACTION_ERROR_STOPPED)?__stopAtLine:0)

	glLineWidth(1.0f);
	glTranslated(imgRect.x,imgRect.y,0);
	glPointSize(pointSize);
	
	if(mapType)
	{
		glPixelZoom(zoom,zoom);
		drawImage(&image,0,imgRect.height,ALIGN_ADJUST);
	}
	if(showErrors)
	{
		glBegin(GL_POINTS);
		// B
		if(mapType)
			setColor(INT_GREEN);
		else
			setColor(INT_DGREEN);
		Error *error_samples,*error;
		if(errors)
		{
			for(j=0;j<K;j++)
			{
				error_samples=errors[j];
				for(i=0;i<N;i++)
				{
					error=error_samples+i;
					x=float(B[i].x)+error->x;
					y=float(B[i].y)+error->y;
					if(x>0&&y>0&&x<areaSide&&y<areaSide)
						VERTEX(x,y);
				}
			}
		}
		// R
		for (i=0;i<K;i++)
		{
			error=distribs+i;
			if(model>=MODEL_ITER&&iterErrSquare&&getSquare(error->x,error->y)>iterErrSquare)
				setColor(INT_RED);
			else if(mapType)
				setColor(INT_YELLOW);
			else
				setColor(INT_BLUE);
			x=float(R.x)+error->x;
			y=float(R.y)+error->y;
			if(x>0&&y>0&&x<areaSide&&y<areaSide)
				VERTEX(x,y);
		}
		//
		setColor(INT_BLACK);
		for (i=0;i<N;i++)
			VERTEX(B[i].x,B[i].y);
		VERTEX(R.x,R.y);
		glEnd();
	}
	if(bShowR.checked)
	{
		float x=MAPX(R.x),y=MAPY(R.y);
		setColor(INT_GRAY);
		glBegin(GL_LINES);
		glVertex2f(0.0f, y);
		glVertex2f(imgRect.width, y);
		glVertex2f(x, imgRect.height);
		glVertex2f(x, 0.0f);
		glEnd();
	}
	if(bShowDist.checked)
	{
		if(mapType)
			setColor(INT_WHITE);
		else
			setColor(INT_BLACK);
		glBegin(GL_LINES);
		for (i=0;i<N;i++)
		{
			VERTEX(R.x,R.y);
			VERTEX(B[i].x,B[i].y);
		}
		glEnd();
	}
	if(motionEstim&&maxGradM>0)
	{
		glLineWidth(2.0f);
		if(mapType)
			setColor(INT_YELLOW);
		else
			setColor(INT_ORANGE);
		glBegin(GL_LINES);
		zoomG=float(0.1*areaSide/maxGradM);
		for (i=0;i<N;i++)
		{
			VERTEX(B[i].x,B[i].y);
			VERTEX(B[i].x+zoomG*gradM[i].x,B[i].y+zoomG*gradM[i].y);
		}
		glEnd();
		glLineWidth(1.0f);
	}
	if(bTrajectCust.checked&&trajectory.length)
	{
		setColor(trajectColor);
		glLineWidth(2.0f);
		PointObject obj;
		glBegin(GL_LINE_STRIP);
		for(obj=trajectory.first;obj;obj=obj->next)
			VERTEX(obj->value.x,obj->value.y);
		glEnd();
		glLineWidth(1.0f);
	}
	if(bRoute.checked)
	{
		if(!processing&&route.length)
		{
			IterObject obj;
			int ci;
			DWORD color;
			
			glLineWidth(2.0f);
			glBegin(GL_LINE_STRIP);
			color=(mapType)?routeColor:INT_BLUE;
			setColor(color);
			VERTEX(p[POS_START].x,p[POS_START].y);
			for(ci=1,obj=route.first;obj;obj=obj->next,ci++)
			{
				if(mapType)
				{
					switch(ci%4)
					{
						case 1:color=colorMix(routeColor,INT_RED);break;
						case 2:color=colorMix(routeColor,INT_BLUE);break;
						case 3:color=colorMix(routeColor,INT_GREEN);break;
						default:color=routeColor;break;
					}
					setColor(color);
				}
				VERTEX(obj->value.pos.x,obj->value.pos.y);
			}
			glEnd();
			glLineWidth(1.0f);
		}
	}
	if(mapType&&section&&mapSection&&graph==GRAPH_SECT)
	{
		if(fg.graph.showMark&&fg.graph.markIndex>0&&(mark.x||mark.y))
		{
			setColor(INT_WBLUE);
			glBegin(GL_LINES);
			VERTEX(0,mark.y);
			VERTEX(areaSide,mark.y);
			VERTEX(mark.x,0);
			VERTEX(mark.x,areaSide);
			glEnd();
		}
		glPushAttrib(GL_LINE_BIT);
		glLineWidth(2.0);
		setColor(INT_WHITE);
		glBegin(GL_LINES);
		VERTEX(sectStart.x, sectStart.y);
		VERTEX(sectEnd.x, sectEnd.y);
		glEnd();
		setColor(INT_BLUE);
		setLineStipple(TRUE,0xf0f0);
		glBegin(GL_LINES);
		VERTEX(sectStart.x, sectStart.y);
		VERTEX(sectEnd.x, sectEnd.y);
		glEnd();
		glPopAttrib();
	}
	float h=(float)ABS(fontSmall.height),w;
	if(chInfo.getSelectedIndex()&&(!mapType||!processing))
	{
		int fillColor;
		Point *b;
		fontSmall.color=INT_BLACK;
		fillColor=INT_WHITE;
		float x,y;
		for (i=-1;i<N;i++)
		{
			b=(i>=0)?(B+i):&R;
			x=MAPX(b->x);
			y=MAPY(b->y);
			//fillColor=(i==active)?INT_YELLOW:INT_WHITE;
			graphText="";
			graphText1="";
			fontSmall.y=y+1.0f;
			y-=h-4.0f;
			switch(chInfo.getSelectedIndex())
			{
			case INFO_NUM:
				if(i>=0)
					graphText=format.toString(i+1);
				else if(bShowDist.checked)
					graphText=_T("?");
				break;
			case INFO_COOR:
				graphText=format.toString(b->x,_T("%.2lf"));
				graphText1=format.toString(b->y,_T("%.2lf"));
				break;
			case INFO_DIST:
				if(i>=0)
					graphText=format.toString(Lr[i],_T("%.3lf"));
				break;
			case INFO_TRANS:
				if(i>=0)
					graphText=format.toString(Lr[i]*sqrt(2*M[i]),_T("%.3lf"));
				break;
			case INFO_MASS:
				if(i>=0)
					graphText=format.toString(M[i],_T("%.1e"));
				break;
			case INFO_GRADM:
				if(i>=0)
					graphText=format.toString(gradM[i].dist(),_T("%.1e"));
				break;
			case INFO_OPT:
				if(i>=0&&optim&&model>=MODEL_ITER)
				{
					graphText=format.toString(O[i],_T("%.4lf"));
				}
				break;
			}
			if(graphText1.length())
			{
				w=3.0f+maxval(fontSmall.getTextWidth(graphText),fontSmall.getTextWidth(graphText1));
				x-=w*0.5f;
				fontSmall.x=x+1.0f;
				fillBox(x-1.0f,y,w,2*h,fillColor);
				drawBox(x-1.0f,y,w,2*h,INT_BLACK);
				fontSmall.print(graphText);
				fontSmall.y+=h;
				fontSmall.print(graphText1);
			}
			else if(graphText.length())
			{
				w=3.0f+fontSmall.getTextWidth(graphText);
				x-=w*0.5f;
				fontSmall.x=x+1.0f;
				fillBox(x-1.0f,y,w,h,fillColor);
				drawBox(x-1.0f,y,w,h,INT_BLACK);
				fontSmall.print(graphText);
			}
		}
	}

	fontSmall.color=INT_WHITE;
	drawCond[POS_TARGET]=bShowTarget.checked;
	drawCond[POS_START]=bShowIterStart.checked;
	drawCond[POS_END]=bShowIterEnd.checked;
	drawCond[POS_ERRMIN]=bShowErrMin.checked&&cog_ok;
	drawCond[POS_ITER]=bShowIter.checked;
	drawCond[POS_LEVMIN]=bShowMin.checked&&levmin!=levmax;
	drawCond[POS_LEVMAX]=bShowMax.checked&&levmin!=levmax;
	for(i=0;i<POS_COUNT;i++)
	{
		if(drawCond[i])
		{
			text=nameShow[i];
			w=3.0f+fontSmall.getTextWidth(text);
			x=MAPX(p[i].x);
			y=MAPY(p[i].y);
			if(i>POS_ITER)
			{
				left=imgRect.width-w;
				start=right=imgRect.width;
			}
			else
			{
				start=left=0;
				right=w;
			}
			fontSmall.x=left+1.5f;
			fontSmall.y=y-3.0f;
		
			setColor(INT_BLACK);
			glBegin(GL_QUADS);
				glVertex2f(left,y);
				glVertex2f(left,y-h);
				glVertex2f(right,y-h);
				glVertex2f(right,y);
			glEnd();
			glBegin(GL_LINES);
				glVertex2f(start,y+1.0f);
				glVertex2f(x,y+1.0f);
				setColor(INT_WHITE);
				glVertex2f(start,y);
				glVertex2f(x,y);
			glEnd();
			fontSmall.print(text);
		}
	}
	setColor(INT_BLACK);
	glBegin(GL_LINE_LOOP);
		glVertex2f(-1.0f, 0.0f);
		glVertex2f(imgRect.width, 0.0f);
		glVertex2f(imgRect.width, imgRect.height+1.0f);
		glVertex2f(-1.0f, imgRect.height+1.0f);
	glEnd();
}
BOOL Trilat::load(CSTR filename)
{
	int i;
	InitInfo info;
	StatBeacon *sb;
	Point tmp;
	String str(64);
	BOOL loaded=info.read(filename);

	PRINT(_T("Loading %s\n"),filename);

	if(loaded)
		tbFile.setText(filename);
	window.left=info.getInt(_T("left"),window.left);
	window.top=info.getInt(_T("top"),window.top);
	window.width=info.getInt(_T("width"),window.width);
	window.height=info.getInt(_T("height"),window.height);
	resetWindow();
	setOrder(		info.getInt(_T("beacons.order"),0));
	setImage(		info.getInt(_T("map.image"),DEF_IMG_SIZE),
					info.getInt(_T("map.area"),DEF_AREA_SIZE));
	setArray(		info.getInt(_T("beacons.count"),DEF_N),
					info.getInt(_T("errors.samples"),DEF_K),
					info.getInt(_T("errors.keep"),1));
	setModel(		info.getInt(_T("model"),MODEL_S_TOF));
	setStat(		info.getDouble(_T("errors.stdevXY"),DEF_STDEV_XY),
					info.getDouble(_T("errors.correl"),DEF_CORREL_XY),
					info.getDouble(_T("errors.stdevL"),DEF_STDEV_L),
					info.getDouble(_T("errors.MP"),DEF_STDEV_MP),
					info.getInt(_T("errors.randev"),0));
	setOptim(		info.getInt(_T("errors.optim"),OPT_TRACE));
	buildStat();
	chInfo.setSelectedIndex(info.getInt(_T("beacons.display"),INFO_NUM));
	R.set(			info.getDouble(_T("x"),0.5*areaSide),
					info.getDouble(_T("y"),0.5*areaSide));
	p[POS_START].set(info.getDouble(_T("iter.start.x"),0),
					info.getDouble(_T("iter.start.y"),0));
	showPos();
	tbCapture.setText(info.get(_T("file.capture"),_T("capture")));
	if(loaded)
	{
		for(i=0;i<N;i++)
		{
			str.print(_T("beacon[%d]."),i+1);
			sb=&stats[i];
			B[i].x=info.getDouble(str+_T("x"),0);
			B[i].y=info.getDouble(str+_T("y"),0);
			sb->devx=info.getDouble(str+_T("dev.x"),0.5*SQRT2*stdevXY);
			sb->devy=info.getDouble(str+_T("dev.y"),0.5*SQRT2*stdevXY);
			sb->devl=info.getDouble(str+_T("dev.l"),stdevL);
			sb->corxy=info.getDouble(str+_T("corxy"),correlXY);
			sb->meanl=info.getDouble(str+_T("mean.l"),meanL);
		}
	}
	updateStats();
	setGraph(		info.getInt(_T("graph"),GRAPH_SECT),
					info.getInt(_T("graph.points"),100));

	mapErrorColor=info.getInt(_T("map.error.color"),INT_RED,_T("%X"));
	mapThresColor=info.getInt(_T("map.thres.color"),INT_CYAN,_T("%X"));
	bMapLims.checked=info.getInt(_T("map.lim"),FALSE);
	setMapLims(		info.getDouble(_T("map.min"),0),
					info.getDouble(_T("map.max"),0));

	trajectColor=info.getInt(_T("traject.color"),INT_BLUE,_T("%X"));
	routeColor=info.getInt(_T("motion.route.color"),INT_YELLOW,_T("%X"));
	adjustGraph(	info.getInt(_T("graph.grid.x"),3),
					info.getInt(_T("graph.grid.y"),7),
					info.getInt(_T("graph.prec.x"),3),
					info.getInt(_T("graph.prec.y"),3),
					info.getDouble(_T("graph.marge.x"),15),
					info.getDouble(_T("graph.marge.y"),75),
					info.getDouble(_T("graph.scale.x"),1),
					info.getDouble(_T("graph.scale.y"),1),
					info.getInt(_T("graph.smooth"),1),
					info.getInt(_T("graph.symbol.auto"),1),
					info.getInt(_T("graph.log"),0),
					info.getInt(_T("graph.exp"),1),
					info.getInt(_T("graph.toolbox"),1));
	
	for(i=0;i<CHNL_COUNT;i++)
	{
		Channel *chnl=channels[i];
		ChnlInfo *chnlInfo=chnlInfos+i;
		str.print(_T("channels[%s]."),(CSTR)(chnlInfo->name));
		if(chnlInfo->x>=0&&chnlInfo->x<CHNL_COUNT)
			chnl->arg=chnlInfos[chnlInfo->x].name;
		chnl->color=info.getInt(str+_T("color"),chnlInfo->color,_T("%X"));
		chnl->thick=(float)info.getDouble(str+_T("thick"),chnlInfo->thick);
		chnl->hidden=info.getInt(str+_T("hidden"),chnlInfo->hidden);
		chnl->setStipple((GLushort)info.getInt(str+_T("pattern"),chnlInfo->pattern,_T("%X")));
		chnl->setSymbol(info.get(str+_T("sym"),chnlInfo->sym),
						info.getInt(str+_T("symIndex"),-1),
						(float)info.getDouble(str+_T("symOffsetX"),0),
						(float)info.getDouble(str+_T("symOffsetY"),0));
	}
	fg.graph.resolveNames(TRUE);

	allowParabEstim(info.getInt(_T("parabEstim"),0));
	allowMotionEstim(info.getInt(_T("motion.estim"),0));
	setMotionTarget(info.getDouble(_T("motion.target.x"),DEF_TARGET_X),
					info.getDouble(_T("motion.target.y"),DEF_TARGET_Y));
	chPlan.setSelectedIndex(info.getInt(_T("motion.plan.type"),DEF_PLAN_TYPE));
	setMotionParams(info.getDouble(_T("motion.region.equal"),DEF_EQUAL_REGION),
					info.getDouble(_T("motion.region.critic"),DEF_CRITIC_REGION),
					info.getDouble(_T("motion.region.active"),DEF_ACTIVE_REGION),
					info.getDouble(_T("motion.speed.reduct"),DEF_SPEED_REDUCT));
	criticSprodStop=info.getDouble(_T("motion.sprod.stop"),DEF_CRITIC_SPROD_STOP);
	criticSprodDir=info.getDouble(_T("motion.sprod.dir"),DEF_CRITIC_SPROD_DIR);
	bShowTarget.checked=info.getInt(_T("motion.target.show"),FALSE);
	bShowIterStart.checked=info.getInt(_T("iter.start.show"),FALSE);
	bShowIterEnd.checked=info.getInt(_T("iter.end.show"),FALSE);
	custStart.x=info.getDouble(_T("section.custom.start.x"),custStart.x);
	custStart.y=info.getDouble(_T("section.custom.start.y"),custStart.y);
	custEnd.x=info.getDouble(_T("section.custom.end.x"),custEnd.x);
	custEnd.y=info.getDouble(_T("section.custom.end.y"),custEnd.y);
	setSection(		info.getInt(_T("section"),DEF_SECT),
					info.getInt(_T("section.lsm"),3));
	setIterParams(	info.getDouble(_T("iter.step.min"),DEF_MIN_ITER_STEP),
					info.getInt(_T("iter.count"),DEF_MAX_ITER_COUNT),
					info.getDouble(_T("iter.grad.min"),DEF_MIN_ITER_GRAD),
					info.getDouble(_T("iter.step.reduct"),DEF_STEP_REDUCT),
					info.getDouble(_T("iter.step.grow"),DEF_STEP_GROW),
					info.getDouble(_T("iter.errDist"),0));
	bShowIter.checked=info.getInt(_T("iter.show"),0);
	p[POS_ITER].set(info.getDouble(_T("iter.x"),0.5*areaSide),
					info.getDouble(_T("iter.y"),0.5*areaSide));
	iterIndex=		info.getInt(_T("iter.index"),-1);
	bRoute.checked=info.getInt(_T("motion.route.show"),TRUE);
	setThreshold(	info.getDouble(_T("map.thres.inf"),0),
					info.getDouble(_T("map.thres.sup"),0));
	setMap(			info.getInt(_T("map"),MAP_EST_VAR),
					info.getInt(_T("map.arg"),0));
	bShowErrors.checked=info.getInt(_T("beacons.distrib.show"),1);
	bShowR.checked=info.getInt(_T("R.show"),1);
	bShowDist.checked=info.getInt(_T("beacons.rays.show"),1);
	bShowMin.checked=info.getInt(_T("map.min.show"),0);
	bShowMax.checked=info.getInt(_T("map.max.show"),0);
	bShowErrMin.checked=info.getInt(_T("map.estMinErr.show"),1);
	bNotify.checked=info.getInt(_T("notify"),1);
	
	if(loaded)
	{
		Tokenizer tokPoint,tCoor;
		tokPoint.set(info.get(_T("traject")),_T(";"));
		if(tokPoint.length)
		{
			Tokenizer::Object tObjPoint;
			trajectory.destroy();
			for(tObjPoint=tokPoint.first;tObjPoint;tObjPoint=tObjPoint->next)
			{
				tCoor.set(tObjPoint->value,_T(","));
				if(tCoor.length>1)
				{
					tmp.x=format.toDouble(tCoor.first->value);
					tmp.y=format.toDouble(tCoor.first->next->value);
					trajectory.add(tmp);
				}
			}
			bTrajectCust.checked=trajectory.length>0;
		}
	}
		
	frame3D=info.getInt(_T("3D.frame"),TRUE);
	box3D=info.getInt(_T("3D.box"),TRUE);
	xyz3D=info.getInt(_T("3D.xyz"),TRUE);
	values3D=info.getInt(_T("3D.values"),TRUE);
	web3D=info.getInt(_T("3D.web"),TRUE);
	onlyWeb=info.getInt(_T("3D.web.only"),FALSE);
	count3DWeb=info.getInt(_T("3D.web.count"),10);
	color3D=info.getInt(_T("3D.color"),TRUE);
	info3D=info.getInt(_T("3D.info"),TRUE);
	light3D=info.getInt(_T("3D.light"),TRUE);
	smooth3D=info.getInt(_T("3D.smooth"),TRUE);
	minMax3D=info.getInt(_T("3D.minMax"),FALSE);
	angleX=(float)info.getDouble(_T("angle.x"),ANGLE_X);
	angleY=(float)info.getDouble(_T("angle.y"),ANGLE_Y);
	angleZ=(float)info.getDouble(_T("angle.z"),ANGLE_Z);
	distX=(float)info.getDouble(_T("dist.x"),DIST_X);
	distY=(float)info.getDouble(_T("dist.y"),DIST_Y);
	distZ=(float)info.getDouble(_T("dist.z"),DIST_Z);

	//recalc();
	startMap();

	PRINT(_T("Loading done\n"));

	return TRUE;
}
BOOL Trilat::save(CSTR filename)
{
	int i;
	InitInfo info;
	StatBeacon *sb;
	String str(64);

	info.addInt(	_T("left"),window.left);
	info.addInt(	_T("top"),window.top);
	info.addInt(	_T("width"),window.width);
	info.addInt(	_T("height"),window.height);
	info.addDouble(	_T("x"),R.x);
	info.addDouble(	_T("y"),R.y);
	info.addInt(	_T("model"),model);
	info.addInt(	_T("parabEstim"),parabEstim);
	info.addInt(	_T("notify"),bNotify.checked);
	
	info.add(		_T("file.capture"),tbCapture.text);

	info.addInt(	_T("beacons.count"),N);
	info.addInt(	_T("beacons.distrib.show"),bShowErrors.checked);
	info.addInt(	_T("R.show"),bShowR.checked);
	info.addInt(	_T("beacons.rays.show"),bShowDist.checked);
	info.addInt(	_T("beacons.order"),chOrder.getSelectedIndex());
	info.addInt(	_T("beacons.display"),chInfo.getSelectedIndex());

	info.addInt(	_T("errors.samples"),K);
	info.addInt(	_T("errors.keep"),keepErrors);
	info.addDouble(	_T("errors.stdevXY"),stdevXY);
	info.addDouble(	_T("errors.correl"),correlXY);
	info.addDouble(	_T("errors.stdevL"),stdevL);
	info.addDouble(	_T("errors.MP"),meanL);
	info.addInt(	_T("errors.randev"),randev);
	info.addInt(	_T("errors.optim"),optim);
	
	info.addInt(	_T("map"),map);
	info.addInt(	_T("map.area"),areaSide);
	info.addInt(	_T("map.image"),imgSide);
	info.addInt(	_T("map.arg"),mapArg);
	info.addInt(	_T("map.error.color"),mapErrorColor,_T("%X"));
	info.addInt(	_T("map.thres.color"),mapThresColor,_T("%X"));
	info.addDouble(	_T("map.thres.inf"),thresInf);
	info.addDouble(	_T("map.thres.sup"),thresSup);
	info.addInt(	_T("map.lim"),bMapLims.checked);
	info.addDouble(	_T("map.min"),mapmin);
	info.addDouble(	_T("map.max"),mapmax);
	info.addInt(	_T("map.min.show"),bShowMin.checked);
	info.addInt(	_T("map.max.show"),bShowMax.checked);
	info.addInt(	_T("map.estMinErr.show"),bShowErrMin.checked);

	info.addInt(	_T("graph"),graph);
	info.addInt(	_T("graph.points"),points);
	info.addInt(	_T("graph.grid.x"),fg.graph.grid.x);
	info.addInt(	_T("graph.grid.y"),fg.graph.grid.y);
	info.addInt(	_T("graph.prec.x"),fg.graph.formatX.precision);
	info.addInt(	_T("graph.prec.y"),fg.graph.formatY.precision);
	info.addDouble(	_T("graph.marge.x"),fg.graph.marginChannel.x);
	info.addDouble(	_T("graph.marge.y"),fg.graph.marginChannel.y);
	info.addDouble(	_T("graph.scale.x"),fg.graph.scale.x);
	info.addDouble(	_T("graph.scale.y"),fg.graph.scale.y);
	info.addInt(	_T("graph.smooth"),fg.graph.smooth);
	info.addInt(	_T("graph.toolbox"),fg.toolbox);
	info.addInt(	_T("graph.symbol.auto"),fg.graph.symAuto);
	info.addInt(	_T("graph.log"),fg.graph.logscaleX);
	info.addInt(	_T("graph.exp"),fg.graph.formatY.exposant);

	info.addInt(	_T("motion.estim"),motionEstim);
	info.addDouble(	_T("motion.target.x"),p[POS_TARGET].x);
	info.addDouble(	_T("motion.target.y"),p[POS_TARGET].y);
	info.addInt(	_T("motion.target.show"),bShowTarget.checked);
	info.addInt(	_T("motion.plan.type"),chPlan.getSelectedIndex());
	info.addDouble(	_T("motion.region.equal"),lt);
	info.addDouble(	_T("motion.region.critic"),rc);
	info.addDouble(	_T("motion.region.active"),la);
	info.addDouble(	_T("motion.speed.reduct"),sr);
	info.addDouble(	_T("motion.sprod.stop"),criticSprodStop);
	info.addDouble(	_T("motion.sprod.dir"),criticSprodDir);	
	info.addInt(	_T("motion.route.show"),bRoute.checked);
	info.addInt(	_T("motion.route.color"),routeColor,_T("%X"));

	info.addInt(	_T("section"),section);
	info.addDouble(	_T("section.custom.start.x"),custStart.x);
	info.addDouble(	_T("section.custom.start.y"),custStart.y);
	info.addDouble(	_T("section.custom.end.x"),custEnd.x);
	info.addDouble(	_T("section.custom.end.y"),custEnd.y);
	info.addInt(	_T("section.lsm"),vSectCoefs.size);

	info.addDouble(	_T("iter.start.x"),p[POS_START].x);
	info.addDouble(	_T("iter.start.y"),p[POS_START].y);
	info.addInt(	_T("iter.start.show"),bShowIterStart.checked);
	info.addInt(	_T("iter.end.show"),bShowIterEnd.checked);
	info.addDouble(	_T("iter.step.reduct"),stepReduct);
	info.addDouble(	_T("iter.step.grow"),stepGrow);
	info.addDouble(	_T("iter.step.min"),minIterStep);
	info.addInt(	_T("iter.count"),maxIterCount);
	info.addDouble(	_T("iter.grad.min"),minIterGrad);
	info.addInt(	_T("iter.show"),bShowIter.checked);
	info.addDouble(	_T("iter.x"),p[POS_ITER].x);
	info.addDouble(	_T("iter.y"),p[POS_ITER].y);
	info.addInt(	_T("iter.index"),iterIndex);
	info.addDouble(	_T("iter.errDist"),sqrt(iterErrSquare));

	info.addInt(	_T("3D.frame"),frame3D);
	info.addInt(	_T("3D.box"),box3D);
	info.addInt(	_T("3D.xyz"),xyz3D);
	info.addInt(	_T("3D.values"),values3D);
	info.addInt(	_T("3D.web"),web3D);
	info.addInt(	_T("3D.web.only"),onlyWeb);
	info.addInt(	_T("3D.web.count"),count3DWeb);
	info.addInt(	_T("3D.color"),color3D);
	info.addInt(	_T("3D.info"),info3D);
	info.addInt(	_T("3D.minMax"),minMax3D);
	info.addInt(	_T("3D.light"),light3D);
	info.addInt(	_T("3D.smooth"),smooth3D);

	info.addDouble(	_T("angle.x"),angleX);
	info.addDouble(	_T("angle.y"),angleY);
	info.addDouble(	_T("angle.z"),angleZ);

	info.addDouble(	_T("dist.x"),distX);
	info.addDouble(	_T("dist.y"),distY);
	info.addDouble(	_T("dist.z"),distZ);

	for(i=0;i<CHNL_COUNT;i++)
	{
		Channel *chnl=channels[i];
		str.print(_T("channels[%s]."),chnl->getName());
		info.addInt(str+_T("hidden"),chnl->hidden);
		info.addInt(str+_T("color"),chnl->color,_T("%X"));
		info.addDouble(str+_T("thick"),chnl->thick,_T("%.1f"));
		info.addInt(str+_T("pattern"),chnl->stipple_pattern,_T("%X"));
		info.add(str+_T("sym"),chnl->sym);
		info.addInt(str+_T("symIndex"),chnl->symIndex);
		info.addDouble(str+_T("symOffsetX"),chnl->symOffset.x);
		info.addDouble(str+_T("symOffsetY"),chnl->symOffset.y);
	}

	for(i=0;i<N;i++)
	{
		str.print(	_T("beacon[%d]."),i+1);
		sb=&stats[i];
		info.addDouble(str+_T("x"),B[i].x);
		info.addDouble(str+_T("y"),B[i].y);
		info.addDouble(str+_T("dev.x"),sb->devx);
		info.addDouble(str+_T("dev.y"),sb->devy);
		info.addDouble(str+_T("dev.l"),sb->devl);
		info.addDouble(str+_T("corxy"),sb->corxy);
		info.addDouble(str+_T("mean.l"),sb->meanl);
	}

	info.addInt(_T("traject.color"),trajectColor,_T("%X"));
	if(trajectory.length)
	{
		for(PointObject obj=trajectory.first;obj;obj=obj->next)
			str.print(_T("%f,%f;"),obj->value.x,obj->value.y);
		info.add(_T("traject"),str);
	}
	if(!info.write(filename))
		return FALSE;
	tbFile.setText(filename);
	return TRUE;
}
void Trilat::init3D()
{
	frame3D=TRUE;
	box3D=TRUE;
	xyz3D=TRUE;
	values3D=TRUE;
	count3DWeb=10;
	web3D=TRUE;
	onlyWeb=FALSE;
	color3D=TRUE;
	info3D=TRUE;
	minMax3D=FALSE;
	light3D=TRUE;
	smooth3D=TRUE;
	angleX=ANGLE_X;
	angleY=ANGLE_Y;
	angleZ=ANGLE_Z;
	distX=DIST_X;
	distY=DIST_Y;
	distZ=DIST_Z;
}
void Trilat::init()
{
	memcpy(LightAmbient,LightAmbient0,sizeof(LightAmbient));
	memcpy(LightDiffuse,LightDiffuse0,sizeof(LightDiffuse));
	memcpy(LightPosition,LightPosition0,sizeof(LightPosition));
	
	ClearStruct(numStat);
	ClearStruct(estStat);
	ClearStruct(iterOut);
	ClearStruct(iterMass);
	ClearStruct(iterMotion);

	N=0;
	B=NULL;
	actionTime=0;
	active_thread=NULL;
	action_result=ACTION_ERROR_NONE;
	levmin=levmax=levmid=0;
	order=0;
	pending_action=0;
	processing=FALSE;
	ClearStruct(iterOut);
	points=0;
	FGW=0;
	iterErrRatio=0;
	iterIndex=-1;
	parabPending=FALSE;
	pointSize=3.0f;
	progress=1;
	graph=GRAPH_NONE;
	parabOK=parabPending=cog_ok=FALSE;
	detU=0;
	U=mU.setSize(3,1);
	Px=mPx.setSize(2,2);
	Ptmp=mPtmp.setSize(2,2);
	Aquad=mAquad.setSize(3,3);
	Da=mDa.setSize(3,3);
	X=mX.setSize(3,1);
	mapArgChanged=mapChanged=mapNoArg=FALSE;
	useCustArg=FALSE;
	mapType=MAP_TYPE_NONE;
	map=MAP_NONE;
	mapArg=MAP_ARG_POINT;
	mapSection=TRUE;
	itersMin=0;
	itersMmax=0;
	active=0;
	maxStepMass=0;
	__stopAtLine=0;
	activeFont=&fontSmall;
	setThreshold(0,0);
	setIterParams(DEF_MIN_ITER_STEP,DEF_MAX_ITER_COUNT,DEF_MIN_ITER_GRAD,DEF_STEP_REDUCT,DEF_STEP_GROW,0);
	R.x=R.y=0.5*areaSide;
	p[POS_START]=R;
	showPos();
	mode3D=FALSE;
	Directory::getCurrent(currentDir);
	CSTR ini=DEF_INI_FILE;
	if(cmdToken.length>0&&cmdToken[0].length())
		ini=cmdToken[0];
	String tmp = _T("Trilat");
	if(load(ini))
	{
		tmp += '(';
		tmp += ini;
		tmp += ')';
	}
	name=tmp;
}
void Trilat::destroy()
{
	stop(TRUE);
	Directory::setCurrent(currentDir);
	save(DEF_INI_FILE);
	FlatApplication::destroy();
}
BOOL Trilat::create()				
{
	FlatApplication::create();
	display.width=1024;
	display.height=768;
	display.bpp=32;
	allowSleep=TRUE;
	clearColor=INT_WHITE;
	//setProcessPriority(HIGH_PRIORITY_CLASS);

	fd.addFilter(_T("ini"),_T("Config files"));

	fontSmall.setFace(_T("Courier New"));
	fontSmall.height=-12;

	fontBig.setFace(_T("Courier New"));
	fontBig.color=INT_BLACK;
	fontBig.height=-28;
	fontBig.bold=TRUE;

	fontMatrix.setFace(_T("Terminal"));
	fontMatrix.height=-12;

	add(&fontSmall);
	add(&fontBig);
	add(&fontMatrix);
	add(&list3D);

	imgRect.y=TEXT_START_Y+TEXT_HEIGHT;
	format.precision=3;

	mapErrorColor=INT_RED;
	mapThresColor=INT_CYAN;
	trajectColor=INT_DBLUE;
	routeColor=INT_YELLOW;

	initUI();

	for(int i=0;i<CHNL_COUNT;i++)
	{
		Channel *chnl=fg.graph.addChannel(chnlInfos[i].name);
		if(!chnl)
		{
			message(_T("Error building channels!"));
			return FALSE;
		}
		channels[i]=chnl;
	}

	init();

	PRINT(_T("\n\n--------------------- Application created.\n"));PRINTMEM();

	return TRUE;
}
void Trilat::initUI()
{
	int i;

	fg.bgColor=INT_GRAY;
	fg.graph.clear();
	adjustGraph(3,7,3,6,15,75,1,1,TRUE,TRUE,FALSE,TRUE,TRUE);
	fg.graph.symBox=TRUE;
	fg.graph.margin=10;
	fg.graph.font=&fontMatrix;
	fg.margin.set(10,10);
	fg.font=&fontSmall;
	fg.chnlReadLines=4;
	fg.fontColor=INT_BLACK;
	//fg.format.precision=6;
	fg.location.set(20,(float)imgRect.y);
	fg.scroll.setRatio(1);
	add(&fg);

	bArray.text=_T("Setup");
	bArray.borderLine=1;
	bArray.borderColor=INT_DGRAY;
	bArray.activeLine=2;
	bArray.font=&fontSmall;
	bArray.fontColor=INT_BLACK;
	bArray.margin.set(5,1);
	bArray.bgColor=0xFFDDDDDD;
	bArray.overColor=INT_WHITE;
	bArray.pressColor=INT_WCYAN;
	bArray.adjustSize=TRUE;
	bArray.location.set(20,10);
	add(&bArray);

	lCount.text=_T("Count");
	lCount.fontColor=INT_DGRAY;
	lCount.font=&fontSmall;
	lCount.margin.set(5,1);
	lCount.adjustSize=TRUE;
	add(&lCount,DIR_RIGHT,5,0,&bArray);

	tbCount.borderLine=1;
	tbCount.borderColor=INT_DGRAY;
	tbCount.activeLine=2;
	tbCount.font=&fontSmall;
	tbCount.fontColor=INT_BLACK;
	tbCount.margin.set(2,1);
	tbCount.format.precision=3;
	tbCount.size.x=25;
	add(&tbCount,DIR_RIGHT,0,0,&lCount);

	lSamples.text=_T("Samples");
	lSamples.copy(lCount);
	add(&lSamples,DIR_RIGHT,5,0,&tbCount);

	tbSamples.copy(tbCount);
	tbSamples.size.x=60;
	add(&tbSamples,DIR_RIGHT,0,0,&lSamples);

	bKeepErrors.text=_T("Keep");
	bKeepErrors.copy(bArray);
	bKeepErrors.checkable=TRUE;
	bKeepErrors.checked=TRUE;
	add(&bKeepErrors,DIR_RIGHT,5,0,&tbSamples);

	lModel.text=_T("Model");
	lModel.copy(lCount);
	add(&lModel,DIR_RIGHT,5,0,&bKeepErrors);

	chModel.alignx=DIR_LEFT;
	chModel.margin.set(10,1);
	chModel.bgColor=INT_WHITE;
	chModel.fontColor=INT_BLACK;
	chModel.font=&fontSmall;
	for(i=0;i<MODEL_COUNT;i++)
		chModel.add(nameModels[i],i);
	add(&chModel,DIR_RIGHT,0,0,&lModel);

	bParabEstim.text=_T("Parab");
	bParabEstim.copy(bKeepErrors);
	add(&bParabEstim,DIR_RIGHT,5,0,&chModel);

	bWinSize.text=_T("Win");
	bWinSize.copy(bArray);
	add(&bWinSize,DIR_RIGHT,10);

	tbWinSizeX.copy(tbCount);
	tbWinSizeX.size.x=35;
	add(&tbWinSizeX,DIR_RIGHT,2);

	tbWinSizeY.copy(tbWinSizeX);
	add(&tbWinSizeY,DIR_RIGHT,2);

	lGraph.text=_T("Graph");
	lGraph.copy(lCount);
	add(&lGraph,DIR_RIGHT,10);

	chGraph.copy(chModel);
	for(i=0;i<GRAPH_COUNT;i++)
		chGraph.add(nameGraphs[i],i);
	add(&chGraph,DIR_RIGHT,0,0,&lGraph);

	tbPoints.copy(tbCount);
	tbPoints.size.x=35;
	add(&tbPoints,DIR_RIGHT,5,0,&chGraph);

	lSection.text=_T("Section:");
	lSection.copy(lCount);
	add(&lSection,DIR_RIGHT,5);

	chSection.copy(chModel);
	for(i=0;i<SECT_COUNT;i++)
		chSection.add(nameSect[i],i);
	add(&chSection,DIR_RIGHT,0,0,&lSection);

	bLSM.text=_T("LSM");
	bLSM.copy(bArray);
	add(&bLSM,DIR_RIGHT,10,0,&chSection);

	tbLSM.copy(tbCount);
	tbLSM.size.x=20;
	add(&tbLSM,DIR_RIGHT,2,0,&bLSM);

/*************************************************/
	
	bImage.text=_T("Area");
	bImage.copy(bArray);
	add(&bImage,DIR_DOWN,0,5,&bArray);

	lArea.text=_T("Real:");
	lArea.copy(lCount);
	add(&lArea,DIR_RIGHT,5,0,&bImage);

	tbArea.copy(tbCount);
	add(&tbArea,DIR_RIGHT,0,0,&lArea);

	lImage.text=_T("Image:");
	lImage.copy(lCount);
	add(&lImage,DIR_RIGHT,5,0,&tbArea);

	tbImage.copy(tbCount);
	add(&tbImage,DIR_RIGHT,0,0,&lImage);

	bShow3D.text=_T("Show3D");
	bShow3D.copy(bArray);
	bShow3D.code=VK_F2;
	bShow3D.pressColor=INT_WYELLOW;
	add(&bShow3D,DIR_RIGHT,5,0,&tbImage);

	bStat.text=_T("Stat");
	bStat.copy(bArray);
	add(&bStat,DIR_RIGHT,10,0,&bShow3D);

	lStdev.text=_T("Dev:");
	lStdev.copy(lCount);
	add(&lStdev,DIR_RIGHT,5,0,&bStat);

	lStdevXY.text=_T("XY");
	lStdevXY.copy(lCount);
	add(&lStdevXY,DIR_RIGHT,2,0,&lStdev);

	tbStdevXY.copy(tbCount);
	tbStdevXY.size.x=45;
	add(&tbStdevXY,DIR_RIGHT);
	
	lStdevL.text=_T("L");
	lStdevL.copy(lCount);
	add(&lStdevL,DIR_RIGHT,2);

	tbStdevL.copy(tbStdevXY);
	add(&tbStdevL,DIR_RIGHT);

	lCorrelXY.text=_T("Cor:");
	lCorrelXY.copy(lCount);
	add(&lCorrelXY,DIR_RIGHT,5);
	
	tbCorrXY.copy(tbStdevXY);
	add(&tbCorrXY,DIR_RIGHT);

	lMeanL.text=_T("MP:");
	lMeanL.copy(lCount);
	add(&lMeanL,DIR_RIGHT,2);

	tbMeanL.copy(tbStdevXY);
	add(&tbMeanL,DIR_RIGHT);
	
	bRandev.text=_T("Rand");
	bRandev.copy(bParabEstim);
	add(&bRandev,DIR_RIGHT,5);

	lOpt.text=_T("Opt");
	lOpt.copy(lCount);
	add(&lOpt,DIR_RIGHT,2);

	chOpt.copy(chModel);
	for(i=0;i<OPT_COUNT;i++)
		chOpt.add(nameOpts[i],i);
	add(&chOpt,DIR_RIGHT,0,0,&lOpt);

	lOrder.text=_T("Order");
	lOrder.copy(lCount);
	add(&lOrder,DIR_RIGHT,10,0,&chOpt);

	chOrder.copy(chModel);
	for(i=0;i<ORDER_COUNT;i++)
		chOrder.add(nameOrder[i],i);
	add(&chOrder,DIR_RIGHT,0,0,&lOrder);

/*************************************************/

	lMap.text=_T("Map");
	lMap.copy(lCount);
	add(&lMap,DIR_DOWN,0,5,&bImage);

	chMap.copy(chModel);
	for(i=0;i<MAP_COUNT;i++)
		chMap.add(nameMaps[i],i);
	add(&chMap,DIR_RIGHT,0,0,&lMap);

	chVar.copy(chModel);
	for(i=0;i<MAP_ARG_COUNT;i++)
		chVar.add(nameVars[i]);
	add(&chVar,DIR_RIGHT,5,0,&chMap);

	bNotify.text=_T("Notify");
	bNotify.copy(bParabEstim);
	bNotify.checked=TRUE;
	add(&bNotify,DIR_RIGHT,5,0,&chVar);

	bThreshold.text=_T("Thres");
	bThreshold.copy(bArray);
	add(&bThreshold,DIR_RIGHT,10,0,&bNotify);

	lInf.text=_T("Inf:");
	lInf.copy(lCount);
	add(&lInf,DIR_RIGHT,5,0,&bThreshold);

	tbInf.copy(tbStdevXY);
	add(&tbInf,DIR_RIGHT,0,0,&lInf);

	lSup.text=_T("Sup:");
	lSup.copy(lInf);
	add(&lSup,DIR_RIGHT,5,0,&tbInf);

	tbSup.copy(tbInf);
	add(&tbSup,DIR_RIGHT,0,0,&lSup);

	bMapLims.text=_T("Lim");
	bMapLims.copy(bParabEstim);
	add(&bMapLims,DIR_RIGHT,10,0,&tbSup);

	tbMapMin.copy(tbStdevXY);
	tbMapMin.size.x=105;
	tbMapMin.format.precision=-1;
	tbMapMin.format.sign='+';
	tbMapMin.format.exposant=TRUE;
	add(&tbMapMin,DIR_RIGHT,3,0,&bMapLims);

	tbMapMax.copy(tbMapMin);
	add(&tbMapMax,DIR_RIGHT,2,0,&tbMapMin);

	bScript.text=_T("Script");
	bScript.copy(bArray);
	add(&bScript,DIR_RIGHT,15,0,&tbMapMax);

	chScript.copy(chModel);
	for(i=0;i<SCRIPT_COUNT;i++)
		chScript.add(nameScripts[i],i);
	add(&chScript,DIR_RIGHT,2,0,&bScript);

/*************************************************/

	bIter.text=_T("Iter");
	bIter.copy(bArray);
	add(&bIter,DIR_DOWN,0,5,&lMap);
	
	lMinIterStep.text=_T("Step");
	lMinIterStep.copy(lCount);
	add(&lMinIterStep,DIR_RIGHT,0,0,&bIter);

	tbMinIterStep.copy(tbStdevXY);
	tbMinIterStep.size.x=55;
	tbMinIterStep.format.precision=0;
	tbMinIterStep.format.exposant=TRUE;
	add(&tbMinIterStep,DIR_RIGHT,0,0,&lMinIterStep);

	lMaxIterCount.text=_T("Count");
	lMaxIterCount.copy(lCount);
	add(&lMaxIterCount,DIR_RIGHT,0,0,&tbMinIterStep);

	tbMaxIterCount.copy(tbMinIterStep);
	add(&tbMaxIterCount,DIR_RIGHT,0,0,&lMaxIterCount);
	
	lMinIterGrad.text=_T("Grad");
	lMinIterGrad.copy(lCount);
	add(&lMinIterGrad,DIR_RIGHT,0,0,&tbMaxIterCount);

	tbMinIterGrad.copy(tbMinIterStep);
	add(&tbMinIterGrad,DIR_RIGHT,0,0,&lMinIterGrad);

	lStepParams.text=_T("Red/Grow");
	lStepParams.copy(lCount);
	add(&lStepParams,DIR_RIGHT,0,0,&tbMinIterGrad);

	tbStepReduct.copy(tbCount);
	tbStepReduct.size.x=55;
	add(&tbStepReduct,DIR_RIGHT,0,0,&lStepParams);

	tbStepGrow.copy(tbStepReduct);
	add(&tbStepGrow,DIR_RIGHT,2,0,&tbStepReduct);

	lErrDist.text=_T("ErrDist");
	lErrDist.copy(lCount);
	add(&lErrDist,DIR_RIGHT);

	tbErrDist.copy(tbStepReduct);
	tbErrDist.size.x=45;
	add(&tbErrDist,DIR_RIGHT);

	bIterStart.text=_T("Start");
	bIterStart.copy(bArray);
	add(&bIterStart,DIR_RIGHT,5);

	tbIterStartX.copy(tbStdevXY);
	tbIterStartX.size.x=45;
	add(&tbIterStartX,DIR_RIGHT,3,0,&bIterStart);

	tbIterStartY.copy(tbIterStartX);
	add(&tbIterStartY,DIR_RIGHT,2,0,&tbIterStartX);

	bStartToR.text=_T(">");
	bStartToR.copy(bArray);
	add(&bStartToR,DIR_RIGHT,5);

	bRtoStart.text=_T("<");
	bRtoStart.copy(bArray);
	add(&bRtoStart,DIR_RIGHT,2);

	bR.text=_T("R");
	bR.copy(bArray);
	add(&bR,DIR_RIGHT,5);

	tbRx.copy(tbStdevXY);
	tbRx.size.x=45;
	add(&tbRx,DIR_RIGHT,3,0,&bR);

	tbRy.copy(tbRx);
	add(&tbRy,DIR_RIGHT,2,0,&tbRx);

/*************************************************/

	lActive.text=_T("Active:");
	lActive.copy(lCount);
	add(&lActive,DIR_DOWN,0,5,&bIter);

	chActive.copy(chMap);
	add(&chActive,DIR_RIGHT);

	bActiveUpdate.text=_T("Update");
	bActiveUpdate.copy(bArray);
	add(&bActiveUpdate,DIR_RIGHT,5);

	lActiveCoor.text=_T("Coor:");
	lActiveCoor.copy(lCount);
	add(&lActiveCoor,DIR_RIGHT,5);

	tbActiveX.copy(tbCount);
	tbActiveX.size.x=45;
	//tbActiveX.format.precision=1;
	add(&tbActiveX,DIR_RIGHT);

	tbActiveY.copy(tbActiveX);
	add(&tbActiveY,DIR_RIGHT,2);

	lActiveStdev.text=_T("Dev:");
	lActiveStdev.copy(lCount);
	add(&lActiveStdev,DIR_RIGHT,5);

	lActiveStdevX.text=_T("X");
	lActiveStdevX.copy(lActiveStdev);
	add(&lActiveStdevX,DIR_RIGHT,2);

	tbActiveStdevX.copy(tbStdevXY);
	add(&tbActiveStdevX,DIR_RIGHT);

	lActiveStdevY.text=_T("Y");
	lActiveStdevY.copy(lActiveStdev);
	add(&lActiveStdevY,DIR_RIGHT,2);

	tbActiveStdevY.copy(tbActiveStdevX);
	add(&tbActiveStdevY,DIR_RIGHT);

	lActiveStdevL.text=_T("L");
	lActiveStdevL.copy(lActiveStdev);
	add(&lActiveStdevL,DIR_RIGHT,2);

	tbActiveStdevL.copy(tbActiveStdevX);
	add(&tbActiveStdevL,DIR_RIGHT);
	
	lActiveCorrelXY.text=_T("Cor:");
	lActiveCorrelXY.copy(lCount);
	add(&lActiveCorrelXY,DIR_RIGHT,5);

	tbActiveCorrel.copy(tbActiveStdevX);
	add(&tbActiveCorrel,DIR_RIGHT);

	lActiveMeanL.text=_T("MP:");
	lActiveMeanL.copy(lActiveStdev);
	add(&lActiveMeanL,DIR_RIGHT,2);

	tbActiveMeanL.copy(tbActiveStdevX);
	add(&tbActiveMeanL,DIR_RIGHT);

	bActiveSwap.text=_T("Swap");
	bActiveSwap.copy(bArray);
	bActiveSwap.code='S';
	add(&bActiveSwap,DIR_RIGHT,5);

/*************************************************/

	lShow.text=_T("Show:");
	lShow.copy(lCount);
	add(&lShow,DIR_DOWN,0,5,&lActive);

	chInfo.copy(chModel);
	for(i=0;i<INFO_COUNT;i++)
		chInfo.add(nameInfos[i],i);
	add(&chInfo,DIR_RIGHT,0,0,&lShow);

	bShowDist.text=_T("Dist");
	bShowDist.copy(bParabEstim);
	bShowDist.checked=TRUE;
	add(&bShowDist,DIR_RIGHT,5,0,&chInfo);

	bShowR.text=_T("R");
	bShowR.copy(bShowDist);
	add(&bShowR,DIR_RIGHT,3,0,&bShowDist);

	bShowErrors.text=_T("Errors");
	bShowErrors.copy(bShowDist);
	bShowErrors.checked=TRUE;
	add(&bShowErrors,DIR_RIGHT,3,0,&bShowR);

	bShowIterStart.text=_T("Start");
	bShowIterStart.copy(bParabEstim);
	add(&bShowIterStart,DIR_RIGHT,3);

	bShowIterEnd.text=_T("End");
	bShowIterEnd.copy(bParabEstim);
	add(&bShowIterEnd,DIR_RIGHT,3);

	bShowTarget.text=_T("Target");
	bShowTarget.copy(bParabEstim);
	add(&bShowTarget,DIR_RIGHT,3);

	bRoute.text=_T("Route");
	bRoute.copy(bParabEstim);
	bRoute.checked=TRUE;
	add(&bRoute,DIR_RIGHT,3,0,&bShowTarget);
	
	bShowIter.text=_T("Iter");
	bShowIter.copy(bShowDist);
	add(&bShowIter,DIR_RIGHT,3,0,&bRoute);

	bShowMin.text=_T("Min");
	bShowMin.copy(bShowDist);
	add(&bShowMin,DIR_RIGHT,3,0,&bShowIter);

	bShowMax.text=_T("Max");
	bShowMax.copy(bShowDist);
	add(&bShowMax,DIR_RIGHT,3,0,&bShowMin);

	bShowErrMin.text=_T("ErrMin");
	bShowErrMin.copy(bParabEstim);
	bShowErrMin.checked=TRUE;
	add(&bShowErrMin,DIR_RIGHT,3,0,&bShowMax);

	bGoTo.text=_T("GoTo");
	bGoTo.copy(bArray);
	add(&bGoTo,DIR_RIGHT,2,0,&bShowErrMin);

	lCust.text=_T("Custom:");
	lCust.copy(lCount);
	add(&lCust,DIR_RIGHT,5,0,&bGoTo);

	bAddCust.text=_T("Add");
	bAddCust.copy(bArray);
	add(&bAddCust,DIR_RIGHT,0,0,&lCust);

	bAllCust.text=_T("All");
	bAllCust.copy(bParabEstim);
	add(&bAllCust,DIR_RIGHT,5,0,&bAddCust);

	bClearCust.text=_T("Clear");
	bClearCust.copy(bArray);
	add(&bClearCust,DIR_RIGHT,5,0,&bAllCust);

	bTrajectCust.text=_T("Traject");
	bTrajectCust.copy(bParabEstim);
	add(&bTrajectCust,DIR_RIGHT,5,0,&bClearCust);

	bFitCust.text=_T("Fit");
	bFitCust.copy(bArray);
	add(&bFitCust,DIR_RIGHT,5,0,&bTrajectCust);

/*************************************************/
	
	lMotion.text=_T("Motion:");
	lMotion.copy(lCount);
	add(&lMotion,DIR_DOWN,0,5,&lShow);

	bMotionEstim.text=_T("Mgrad");
	bMotionEstim.copy(bArray);
	bMotionEstim.checkable=TRUE;
	bMotionEstim.checked=TRUE;
	add(&bMotionEstim,DIR_RIGHT,5,0,&lMotion);

	bMassMax.text=_T("Mmax");
	bMassMax.copy(bArray);
	add(&bMassMax,DIR_RIGHT,5,0,&bMotionEstim);

	bPlan.text=_T("Plan");
	bPlan.copy(bArray);
	add(&bPlan,DIR_RIGHT,10,0,&bMassMax);

	chPlan.copy(chModel);
	for(i=0;i<PLAN_TYPE_COUNT;i++)
		chPlan.add(namePlans[i]);
	add(&chPlan,DIR_RIGHT,3,0,&bPlan);
	
	bShowFirstPass.text=_T("fp");
	bShowFirstPass.copy(bParabEstim);
	add(&bShowFirstPass,DIR_RIGHT,5);

	lMotionEqual.text=_T("lt");
	lMotionEqual.copy(lCount);
	add(&lMotionEqual,DIR_RIGHT,5);

	tbMotionEqual.copy(tbCount);
	tbMotionEqual.size.x=45;
	add(&tbMotionEqual,DIR_RIGHT,0,0,&lMotionEqual);

	lMotionActive.text=_T("la");
	lMotionActive.copy(lCount);
	add(&lMotionActive,DIR_RIGHT,0,0,&tbMotionEqual);

	tbMotionActive.copy(tbMotionEqual);
	add(&tbMotionActive,DIR_RIGHT,0,0,&lMotionActive);

	lSpeedReduct.text=_T("sr");
	lSpeedReduct.copy(lCount);
	add(&lSpeedReduct,DIR_RIGHT,0,0,&tbMotionActive);

	tbSpeedReduct.copy(tbMotionEqual);
	add(&tbSpeedReduct,DIR_RIGHT,0,0,&lSpeedReduct);

	lMotionCritic.text=_T("rc");
	lMotionCritic.copy(lCount);
	add(&lMotionCritic,DIR_RIGHT,0,0,&tbSpeedReduct);

	tbMotionCritic.copy(tbMotionEqual);
	add(&tbMotionCritic,DIR_RIGHT,0,0,&lMotionCritic);

	lTarget.text=_T("Target");
	lTarget.copy(lCount);
	add(&lTarget,DIR_RIGHT,5,0,&tbMotionCritic);

	tbTargetX.copy(tbActiveX);
	add(&tbTargetX,DIR_RIGHT,0,0,&lTarget);

	tbTargetY.copy(tbTargetX);
	add(&tbTargetY,DIR_RIGHT,2,0,&tbTargetX);

	lForce.text=_T("Force");
	lForce.copy(lCount);
	add(&lForce,DIR_RIGHT,5,0,&tbTargetY);

	bForceM.text=_T("M");
	bForceM.copy(bParabEstim);
	bForceM.checked=TRUE;
	add(&bForceM,DIR_RIGHT,0,0,&lForce);

	bForceP.text=_T("mP");
	bForceP.copy(bParabEstim);
	bForceP.checked=TRUE;
	add(&bForceP,DIR_RIGHT,2,0,&bForceM);

	bForceC.text=_T("C");
	bForceC.copy(bParabEstim);
	bForceC.checked=TRUE;
	add(&bForceC,DIR_RIGHT,2,0,&bForceP);

/*************************************************/
	
	bCapture.text=_T("Capture");
	bCapture.copy(bArray);
	bCapture.code=VK_F6;
	add(&bCapture,DIR_DOWN,0,5,&lMotion);

	tbCapture.copy(tbCount);
	tbCapture.size.x=130;
	add(&tbCapture,DIR_RIGHT,5,0,&bCapture);

	chFormat.copy(chMap);
	chFormat.add(_T("jpg"));
	chFormat.add(_T("bmp"));
	chFormat.add(_T("tga"));
	chFormat.setSelectedIndex(0);
	add(&chFormat,DIR_RIGHT,5,0,&tbCapture);

	tbCompr.copy(tbCount);
	tbCompr.text=_T("100");
	tbCompr.size.x=35;
	add(&tbCompr,DIR_RIGHT,5,0,&chFormat);

	bSave.text=_T("Save");
	bSave.copy(bArray);
	add(&bSave,DIR_RIGHT,20,0,&tbCompr);

	bLoad.text=_T("Load");
	bLoad.copy(bArray);
	add(&bLoad,DIR_RIGHT,5,0,&bSave);

	tbFile.copy(tbCount);
	tbFile.text=DEF_INI_FILE;
	tbFile.size.x=160;
	add(&tbFile,DIR_RIGHT,5,0,&bLoad);
	
	bBrowse.text=_T("...");
	bBrowse.copy(bParabEstim);
	bBrowse.checked=TRUE;
	add(&bBrowse,DIR_RIGHT,5,0,&tbFile);

	bDump.text=_T("Dump");
	bDump.copy(bArray);
	add(&bDump,DIR_RIGHT,20,0,&bBrowse);

	tbDump.copy(tbFile);
	tbDump.text=_T("dump.txt");
	add(&tbDump,DIR_RIGHT,5,0,&bDump);

	bConsole.text=_T("Console");
	bConsole.copy(bParabEstim);
	add(&bConsole,DIR_RIGHT,5);
}

}; // namespace Lio

int WINAPI _tWinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, LPTSTR lpCmdLine, int nCmdShow)
{
	TEMP_DEBUG_OUT(_T("debug.txt"));
	BOOL exc=TRUE;
	int result=1,exc_code=0;
	Lio::CSTR exc_msg=NULL;

	#pragma omp parallel
	{
		PRINT("Thread %d/%d\n", omp_get_thread_num()+1, omp_get_num_threads());
	}

	try
	{
		Lio::Trilat app;
		PRINT(_T("\n\n--------------------- Application started.\n"));PRINTMEM();
		result=app.main(hInstance,hPrevInstance,lpCmdLine,nCmdShow);
		exc=FALSE;
	}
	catch(const char * str){exc_msg=str;}
	catch(int n){exc_code=n;}
	catch(...){}
	if(exc)
	{
		PRINT(	_T("\n\n--------------------- Application interrupted by an exception!\nError code: %d\nError msg: %s\n"),
				exc_code,exc_msg?exc_msg:"none");
	}
	PRINT(_T("\n\n--------------------- Application closed.\n"));PRINTMEM(TRUE);
	return result;
}